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内 容 简 介 


本 书 以 目标 跟踪 为 应 用 背景 ， 系 统 阐 述 了 贝 叶 斯 递 推 估计 框架 下 的 非 线性 滤波 理论 和 实现 方法 ， 并 
针对 不 同 的 算法 给 出 了 大 量 的 应 用 实例 。 全书 内 容 共 分 四 大 部 分 、 十 二 章 , 第 一 部 分 为 非 线性 滤波 理论 与 
应 用 基础 (分 两 章 ), 主要 包含 卡尔 曼 滤 波 算法 原理 及 各 种 衍生 形式 、 目 标 跟踪 的 系统 模型 、 性 能 测度 等 ; 
第 二 部 分 为 解析 高 斯 近似 滤波 与 应 用 ( 分 四 章 )， 系 统 阐 述 四 大 类 非 线性 滤波 的 算法 原理 、 实 现 方法 及 改 
进 算 法 ， 具 体 包 括 函 数 近似 法 (扩展 卡尔 受 滤波 、 中 心 差分 卡尔 受 滤波 )、 确 定性 采样 近似 法 ( 不 教 卡尔 
曼 滤波 )、 积 分 近似 法 ( 求 积 卡尔 曼 滤 波 、 求 容积 卡尔 曼 滤 波 ) 及 混合 近似 法 ( 高 斯 和 扩展 卡尔 曼 滤波 、 
高 斯 和 不 敏 卡尔 曼 滤 波 ) 等 ; 第 三 部 分 为 随机 采样 近似 滤波 与 应 用 (分 四 章 )， 详 细 阐 述 四 类 重要 的 随机 
采样 近似 滤波 算法 原理 和 实现 方法 , 主要 包括 标准 粒子 滤波 算法 、 序 贯 重 要 采样 滤波 (扩展 粒子 滤波 、 不 
敏 粒子 滤波 ) 及 改进 算法 、 边 缘 化 粒子 滤波 算法 及 智能 优化 采样 滤波 ( 包括 遗传 粒子 滤波 、 免 疫 粒子 滤波 、 
人 工 鱼 群 粒子 滤波 ) 等 ; 第 四 部 分 为 非 线性 滤波 的 扩展 与 应 用 (分 两 章 )， 重点 闵 述 在 多 传感器 和 多 模型 
两 种 扩展 条 件 下 、 基 于 解析 高 斯 和 随机 采样 两 种 近似 的 滤波 算法 原理 和 实现 方法 ,包括 多 传感器 融合 滤波 、 
多 模型 滤波 等 。 

本 书 可 作为 从 事 非 线性 滤波 、 贝 叶 斯 估计 、 随 机 信号 处 理 、 目 标 跟踪 等 科研 工作 人 员 的 参考 用 书 ， 
也 可 作为 电子 技术 、 信 息 技术 、 自 动 化 技术 等 专业 的 研究 生 用 书 和 补充 教材 。 
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客观 世界 中 绝 大 部 分 物理 系统 都 是 非 线 性 的 ， 这 一 特点 使 经 典 的 线性 滤波 理论 在 
解决 很 多 实际 问题 时 面临 着 巨大 的 挑战 ， 从 而 催生 了 非 线性 滤波 理论 的 研究 。 在 各 种 
现实 需求 的 强劲 牵引 和 计算 机 技术 发 展 的 有 力 推动 下 , 过 去 的 20 年 一 30 年 中 各 种 非 线 
性 滤波 理论 和 方法 相继 涌现 ， 并 被 广泛 应 用 于 雷达 、 通 信 、 导 航 、 控 制 、 金 融和 统计 
等 众多 军事 和 民用 领域 。 

非 线性 滤波 问题 的 本 质 ， 是 利用 含有 噪声 的 观测 量 实现 对 非 线性 随机 系统 状态 的 估 
计 。 解 决 这 一 估计 问题 通常 有 递 推 处 理 和 批 处 理 两 种 方式 ， 由 于 递 推 处 理 方式 既 不 需要 
存储 完整 的 数据 集 ， 也 不 必 重 新 处 理 之 前 已 经 处 理 过 的 数据 ， 因 此 在 实际 应 用 中 兼 具 多 
方面 的 优势 ， 特 别 适 合 实 时 应 用 场合 。 

作为 非 线 性 递 推 滤波 的 典型 代表 ， 扩 展 卡尔 曼 滤 波 (EKF) 方法 因 其 运算 简单 、 方 
便 实现 等 优势 在 过 去 的 几 十 年 中 广 为 使 用 。 但 是 ,一 方面 ， 由 于 经 典 EKF 仅 采 用 一 阶 线 
性 化 近似 ,在 系统 非 线性 程度 较 高 的 条 件 下 估计 性 能 较 差 ， 男 一 方面 ， 经典 EKF 滤波 在 
实现 过 程 中 需要 计算 非 线性 函数 的 雅 各 比 和 矩阵 ( 即 要 求 非 线 性 函数 解析 可 微 )， 且 不 适用 
于 非 高 斯 噪声 条 件 ， 大 大 制约 了 其 应 用 范围 ，EKF 的 这 些 固有 缺点 促使 人 们 不 断 寻 求 新 
的 非 线 性 滤波 方法 。 

本 书 以 目标 跟踪 为 应 用 背景 , 系统 梳理 了 近 20 年 来 非 线 性 滤波 领域 的 重大 理论 和 学 
术 思 想 ， 全 面 阐述 了 各 种 非 线性 滤波 方法 的 理论 基础 、 实 现 方法 及 适用 条 件 等 ， 并 大 量 
融入 了 作者 近年 来 在 这 方面 的 理论 研究 和 实践 成 果 。 

全 书 以 不 同 条 件 下 的 贝 叶 斯 推演 算式 求解 方法 为 主线 ， 将 内 容 为 四 个 部 分 。 第 一 
部 分 为 非 线性 滤波 理论 与 应 用 基础 ， 共 包含 两 章 ， 主 要 介绍 非 线 性 滤波 及 目标 跟踪 应 
用 的 一 些 基础 问题 ;第 二 部 分 为 解析 高 斯 近似 滤波 与 应 用 ， 共 包含 四 章 ， 系 统 痔 述 几 
种 基于 解析 高 斯 近似 (如 函数 近似 法 、 确 定 采 样 近似 法 、 积 分 近似 法 及 混合 近似 法 等 ) 
的 非 线 性 滤波 算法 原理 和 实现 方法 ;第 三 部 分 为 随机 采样 近似 滤波 与 应 用 ， 共 包含 四 
章 ， 详 细 阐 述 基 于 蒙特 卡 罗 模 拟 技术 的 随机 采样 近似 滤波 算法 原理 和 实现 方法 ; 第 四 
部 分 为 非 线 性 滤波 的 扩展 与 应 用 ， 共 包含 两 章 ， 重 点 阐述 非 线性 滤波 理论 在 多 传感器 
和 多 模型 两 种 扩展 条 件 下 的 算法 原理 和 实现 方法 。 

作为 作者 及 其 所 在 科研 团队 多 年 来 在 该 领域 的 研究 和 实践 成 果 总 结 ， 本 书 具 有 以 下 
几 个 显著 特点 : 

系统 性 。 将 书 中 介绍 的 多 种 非 线性 滤波 方法 统一 纳入 到 贝 叶 斯 估计 理论 框架 下 ， 并 
指出 各 自 的 区 别 和 联系 ， 尽 量 使 读者 做 到 不 仅 知 其 然 ， 而 且 知 其 所 以 然 。 

新 疾 性 。 注 重 以 经 典 理论 的 方法 论 为 基础 ， 阐 述 非 线性 滤波 方法 的 发 展 过 程 及 其 最 
新 动态 ， 其 中 部 分 内 容 为 作者 最 新 研究 成 果 。 


HI 


详实 性 。 书 中 给 出 了 大 量 针 对 不 同 应 用 的 计算 实例 ， 这 些 实例 涵盖 了 当前 目标 跟踪 
领域 很 多 新 的 热点 ， 书 中 介绍 的 绝 大 多 数 方 法 都 经 过 作者 的 编程 验证 ， 具 有 很 好 的 可 重 
复 性 。 

针对 性 。 尽 管 书 中 介绍 的 各 种 方法 具有 广泛 用 途 ， 但 作者 主要 以 雷达 和 红外 传感器 
观测 为 侧重 点 讨论 不 同 条 件 下 的 跟踪 应 用 问题 ， 避 免 了 在 内 容 安排 方面 的 泛 而 空 现象 。 

本 书 的 出 版 得 到 了 国防 科学 技术 大 学 研究 计划 (JC08-04-04)、 武 器 装备 预 研 基金 
(xxx0909KG0107) 及 国家 自然 科学 基金 (61002022) 的 资助 。 另 外 , 在 本 书 撰写 过 程 中 ， 
参考 了 国内 外 相关 领域 专家 学 者 的 研究 成 果 ， 在 此 一 并 表达 诚挚 的 谢意 。 

当前 ， 各 种 新 理论 、 新 方法 的 发 展 日 新 月 异 ， 作 为 具有 广泛 应 用 前 景 和 巨大 扩展 潜 
力 的 非 线 性 滤波 理论 也 是 如 此 。 尽 管 作者 试图 将 这 套 理论 方法 以 最 恰当 的 方式 呈现 给 读 
者 ， 但 受 现 有 条 件 和 作者 水 平 所 限 ， 书 中 难免 有 玖 漏 和 欠 妥 之 处 ， 居 请 读者 及 同仁 批评 
指正 。 


作 者 
2012 年 10 月 
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客观 世界 中 绝 大 部 分 问题 都 需要 用 非 线性 系统 来 描述 ， 由 于 这 一 问题 的 普遍 性 ， 使 
得 人 们 对 非 线性 系统 的 建 模 和 估计 研究 随 着 时 间 推 移 不 断 深入 和 拓展 '""。 非 线性 滤波 
的 本 质 是 基于 当前 《或 累积 ) 系统 量 测 ， 实 现 对 非 线 性 系统 当前 状态 的 准确 估计 。 这 里 
的 非 线性 系统 包含 两 层 含义 : 一 是 系统 ， 包 含 系统 状态 随时 间 的 演化 过 程 和 观测 手段 ， 
通常 用 状态 方程 和 观测 方程 来 建 模 ， 二 是 非 线 性 ， 即 状态 方程 和 量 测 方程 至 少 有 一 个 是 
非 线性 的 。 在 本 书 所 讨论 的 系统 中 ， 状 态 方程 用 差分 方程 来 描述 ， 观 测 方程 用 离散 时 间 
观测 模型 来 表示 ， 由 于 考虑 了 过 程 噪 声 和 量 测 噪声 的 影响 ， 因 此 两 者 都 是 统计 模型 。 

对 于 随机 系统 的 估计 问题 ， 贝 叶 斯 推演 方法 提供 了 一 套 严格 、 统 一 的 处 理 框架 ， 该 
方法 通过 利用 所 有 可 能 获取 的 信息 《包括 观测 序列 ) 来 构建 系统 状态 的 后 验 概率 密度 函 
数 (PDF)。 由 于 这 一 PDF 包含 了 所 有 期 望 得 到 的 统计 信息 ， 因 此 可 认为 是 相应 得 到 了 
被 估计 问题 的 完整 的 解 ， 理 论 上 通过 该 PDF 也 能 得 到 系统 状态 〈 在 某 种 准则 下 ) 的 最 优 
估计 及 其 估计 结果 的 性 能 测度 。 

在 很 多 情况 下 (如 一 些 对 实时 性 要 求 高 的 场合 ), 要 求 在 新 的 观测 序列 到 达 时 能 即时 
得 到 状态 的 估计 。 在 这 种 条 件 下 ， 需 要 采用 递 推 滤波 方法 ， 这 就 意味 着 对 所 获取 的 数据 
采用 的 是 序 贯 处 理 而 非 批 处 理 方式 。 在 这 种 处 理 方法 中 ， 不 需要 存储 完整 的 数据 集 ， 也 
不 必 重 新 处 理 之 前 已 经 处 理 过 的 数据 ， 因 此 在 实际 应 用 中 兼 具 多 方面 的 优势 。 递 推 滤波 
主要 通过 两 个 步骤 来 完成 ， 即 预测 与 更 新 : 在 预测 阶段 ， 利 用 状态 方程 实现 系统 状态 由 
当前 时 刻 到 下 一 个 时 刻 PDF 的 前 向 预测 ,在 更 新 阶段 ， 主 要 利用 新 近 的 观测 量 并 结合 贝 
叶 斯 定理 来 修正 状态 的 预测 PDF 从 而 得 到 其 后 验 PDF。 

本 部 分 主要 介绍 非 线性 滤波 及 目标 跟踪 应 用 的 一 些 基础 问题 ， 第 1 章 首先 给 出 了 路 
叶 斯 滤波 的 统一 框架 ， 在 此 基础 上 针对 一 类 特殊 的 〈 线 性、 高 斯 ) 系统 ， 重 点 阐述 了 最 
优 卡尔 曼 滤 波 的 原理 及 其 扩展 应 用 方法 .第 2 章 则 主要 针对 雷达 /红外 目标 跟踪 应 用 背景 ， 
介绍 了 常用 的 系统 模型 、 常 用 的 滤波 性 能 评估 指标 及 理论 误差 下 限 的 计算 方法 等 ， 并 给 
出 了 具体 的 应 用 实例 。 


第 1 章 离散 时 间 最 优 贝 叶 斯 滤波 


贝 叶 斯 滤波 为 随机 系统 的 状态 估计 问题 提供 了 一 套 具 有 数学 上 严格 定义 、 形 式 上 完 
整 表达 的 统一 处 理 框架 ， 这 一 处 理 框 架 给 出 了 状态 估计 问题 最 优 解 的 通用 表达 式 〈 概 念 
解 )。 但 在 大 多 数 情况 下 《〈 尤 其 是 在 非 线性 条 件 下)， 都 无 法 获得 这 种 通用 数学 公式 的 解 
析 解 (或 闭 式 解 )。 不 过 ， 对 于 几 种 特殊 的 系统 ， 闭 式 解 的 形式 不 但 存在 ， 而 且 可 以 方便 
获得 ， 其 中 线性 、 高 斯 系统 就 是 这 样 一 类 特殊 的 重要 系统 。 

本 章 首 先 针对 一 般 的 离散 时 间 系 统 模 型 ， 给 出 了 贝 叶 斯 滤波 的 求解 过 程 及 解 的 通用 
表达 式 ， 重 点 针对 线性 、 高 斯 的 特殊 条 件 ， 在 最 大 后 验 (MAP) 估计 和 最 小 均 方 误差 
(MMSE) 估计 两 种 最 优化 准则 下 详细 推导 了 最 优 卡 尔 曼 (Kalman). 滤波 算法 。 并 结合 
实际 应 用 中 面临 的 一 些 常见 问题 ， 给 出 了 卡尔 曼 滤 波 算法 的 几 种 重要 扩展 形式 ， 包 括 具 
有 外 部 控制 输入 、 含 未 知 统计 特性 噪声 、 带 恒定 系统 参数 及 求 根 实现 等 形式 。 


11 贝 叶 斯 滤波 的 统一 框架 


考虑 下 列 方程 所 描述 的 离散 时 间 非 线性 系统 
x, = f (X1) + W, (1-1) 
yy, = h(x,) + v, (1-2) 
AP: x, e R^ 为 系统 状态 ，f() 和 所 ) 为 已 知 非 线 性 函数 ，y, e R” AEM; w, 和 vw 分 
别 为 过 程 噪声 和 观测 噪声 。 
从 贝 叶 斯 估计 的 方法 论 来 看 , 大 时 刻 状 态 量 x, 的 后 验 密度 能 够 提供 该 时 刻 状 态 完 整 
的 统计 描述 。 滤 波 问题 就 是 在 给 定 观测 mx = Qo Yir 的 条 件 下 得 到 时 变 状 态 量 x, 的 估计 ， 
这 要 求 建立 后 验 状 态 分 布 p(x, | yx) 的 概率 密度 函数 (PDF )。 假 定 初 始 PDF 为 
P(X) Ê P(x |o), ， 则 理论 上 p(xi | ya) 可 通过 以 下 两 步 递 推 计算 求 得 。 
预测 : 假定 -1 时 刻 状 态 的 后 验 PDF 为 p(xi lJa) ， 则 其 一 步 前 向 预测 的 PDF 
可 根据 Chapman-Kolmogorov 方程 计算 为 
p, | yu a)7 fo | X4) POCO a | Yu, (1-3) 
式 中 隐 含 假定 了 pr, |x, )=p(x, | X41) PG |x, ORIS 956459 86838, Ri 
于 描述 式 〈1-1) 中 的 一 阶 马尔 可 夫 过 程 。 
更 新 : 在 获得 p(xi | yu) 的 基础 上 ， 综 合 新 近 的 观测 jy, ， 可 通过 如 下 的 贝 叶 斯 规 
则 计算 x 时 刻 状态 的 后 验 PDF 
p(x, | yu) = p(x, | Ya k) 
= Pk | x Yu) POS Haa) _ POS | X) Pk Hua) (1-4) 
PG. | Yi) PX | ia) 


RP: pO pix)= [PO |x,)p(x, | ya dx, 为 归 一 化 系数 。 
式 (1-3) 和 式 (1-4) 描述 了 一 个 由 丰 -1 时刻 后 验 PDF 向 大 时 刻 后 验 PDF 递 推 的 完 
整 过 程 ， 从 而 构成 了 贝 叶 斯 估计 最 优 解 的 通用 表示 形式 。 通 过 后 验 分 布 p(x, yu) 可 得 
到 不 同 准 则 条 件 下 x, 的 最 优 估 计 总 ， 如 最 小 均 方 误差 (MMSE) 估计 
EAS a Elxi yi = [x oon, | yix Xix, (1-5) 
和 最 大 后 验 (MAP) 估计 


&“ Sargmax p(x, | yy.) (1-6) 


需要 指出 的 是 ， 式 (1-3) 和 式 (1-4) 所 描述 的 只 是 概念 上 的 解 的 形式 ， 在 通常 情 
况 下 往往 难以 得 到 闭 式 解 。 但 在 以 下 几 种 特殊 的 条 件 下 &20， 贝 叶 斯 估计 的 最 优 解 是 存 
在 的 : 

(D X (1-1) AX (1-2) 所 描述 的 是 线性 系统 模型 ， 且 w, 和 vw 为 高 斯 噪声 ， 此 时 
卡尔 曼 滤波 结果 即 为 最 优 估计 。 

(2) 状态 空间 为 离散 值 ， 且 其 状态 个 数 为 固定 有 限 值 ， 此 时 网 格 化 方法 "为 最 优 滤 
波 算法 。 

(3) 一 类 具有 线性 观测 的 特殊 非 线 性 系统 模型 中 ， 此 时 也 可 得 出 最 优 解 的 形式 。 


12 最 优 卡尔 曼 滤 波 


1.2.1 卡尔 曼 滤 波 的 MAP 准则 推导 
在 式 (1-3) P, Æ PX | yu ) 满足 高 斯 分 布 ， 同 时 有 下 列 假设 成 立 : 
(1) wx 和 vw 为 相互 独立 的 零 均值 高 斯 噪声 ， 其 协 方差 分 别 为 @, M R, ; 
(2) fra) A xi 的 线性 函数 ，h(xi ) A x, 的 线性 函数 。 

此 时 式 (1-1) AR (1-2) 可 写 为 


x, = Fx, +W, (1-7) 
y, = Hx, + v, (1-8) 
则 根据 式 (1-4) 可 知 
p(x, | Yi) = PO lx )P(x; ya) cc p(y, | Xy) PX, | ua) (1-9) 
pyr | ia) 
对 式 (1-9) 取 对 数 ， 得 到 式 〈1-6) 的 等 价 算式 为 

条”=argmin 全 In[P(Ox |X) PC | Va) (1-10) 

容易 证 明 ， p(X | yu a4) 和 p(x, |Jnx) 可 以 通过 解析 递 推 求 得 ， 且 满足 高 斯 分 布 ， 即 
P(X | Jua) m N (Ga 554 P.) (1-11) 


po, | Vix) = NA (x, Xo Pa) 
1 


aa 
"MAKETEN 


(1-12) 


其 中 


Šiki = [xps | Ji Xx, = f fures | X4 ix, |PC | 9a Xx 


(1-13) 
= [Fx, P | Vix- dx. = FX, , 
Pig- = E([Fx,_, +w, — FX  |[Fx,_, + w, — FX, , n 
= E(LFGx, , 7 X, 4) + w,][F(x, — 3,4) + w, u (1-14) 
= FE((x 4 — X, 4) 94 3,4) ]F + Elw,w, ) 
= FP, ,F' + 0, 
AP: P, =cov[x, ,]=E[(x,_, 7 X,4)09 4 7 X. ) ] » 
又 因为 
pG, |x,) = N (y,;Hx,, R) 
_ (1-15) 


pee = Hx, ) R” (y, zi Hx, )] 
Jom” det[R] 


于 是 满足 式 〈1-10) 的 x 等 价 于 
Zio - Hx, R^ (y, — Hx,)] Gs -Sea Pal (x, -En )]=0 (1-16) 
k k 


-H'R (y, - Hx,)+ Pj , (x, — rk) = 0 (1-17) 
Pa (x, — Žir) = H'R' (y, - Hx,) 

-H'K (y, - Hx, + H£,,,- H£,41) (1-18) 

-H'R'(y,-H£,,)-H'R H(x, — X41) 
(Pj, + HR Hyx, - X4, )  H' R (y, - HE.) (1-19) 

H-TAEBE Pu i AIR HANER, UPS tH RH sp, FEER (1-195, 得 
E o-XQatURGatH'ROH) HOR (y, — Hi) (1-20) 
TEX 

K, $(B;,*H'RH) HR'-PHR (1-21) 


XA 〈1-21) 进行 适当 的 变形 ， 得 
K, = P'H' R'(HP,, ,H! + RHPH" + R)” 
-P'(H'R'HP,,H' +H'R R(HP,, ,H' + R) 
-P'(H'R'H«Py4 )P,4, H" (HP, ,H' + R)' (1-22) 
= P'(PyB, H'(HP,,H' + Ry' 
= P,, ,H' (HP yH" + R) ' 
同 理 可 得 
P, = cov[x,] = E((x, — €, x, — X,) ) 
= E([x, — X4. - K, (y - Hz, x, — X, — K,(y - HE, 0 (1-23) 
= Pg- Pj HK, — K, HP,, + K, (HP, HT + R)K, 


将 式 〈1-22) 中 的 KK 代入 式 〈1-23)， 并 采用 矩阵 求 首 引 理 〈 见 附录 A) 化 简 ， 得 
P, = Pik- — Pa z+ H ë (HP,, ,H +R)” HP,, , 


e PC EUR (1-24) 
-(Bl,*H'R^Hy!-P 
于 是 有 
P, -P4 4-7 K,HP,, = (I - K,H)P,x_ (1-25) 
p(x, | Vix) = N (x, P.) (1-26) 
由 此 可 得 到 完整 的 卡尔 曼 滤 波 递 推算 式 : 
Xi = FX (1-27) 
Pi = P, F” +Q, (1-28) 
X, = Sip- + K,[y, — Hx] (1-29) 
P, =P 7 K,HP,, , I (1-30) 
S, = HPH" + R, (1-31) 
K, = PH" Se (1-32) 


AP: K 为 卡尔 最 增益 5, 为 新 息 D- Ha) 的 协 方差 矩阵 。 只 要 给 定 初始 
Afr X, = Elx] K P, 2 cov[x,], ERR (1-27) 一 式 (1-300 进行 递 推 计算 ， 即 可 动态 
得 到 状态 量 x, 的 滤波 估计 。 

值得 一 提 的 是 ， 由 于 S,=cov[o,]-P, , P, ,H' -cov[X,, Jay ]-P, ， 实 际 应 用 中 
可 根据 具体 需要 将 卡尔 曼 增益 表示 成 不 同 的 形式 ， 如 


K,-P P (1-33) 
由 此 可 得 式 (1-300. 的 另 一 种 更 新 形式 为 
P, = Pi -K,P,K, (1-34) 
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鉴于 卡尔 曼 算法 原理 的 重要 性 ， 下 面 将 从 另 一 个 角度 一 一 正 交 投影 原理 出 发 对 其 进 
行 再 推导 。 
1. 正 交 投影 原理 
设 X 和 Zz 分 别 为 n, 维 和 nn, 随机 矢量 ，A 为 nxn, 维 矩阵，b 为 n, 维 非 随机 矢量 。 若 
存在 一 个 nn, 维 矢量 t(z) 满足 如 下 几 个 条 件 : 
(1) <(z) |H z 线性 表示 ， 即 存在 
X(z)-Az*b 
(2) 无 偏 性 ， 即 
E[X(z)]=E[x] 
(3) 正 交 性 ， 即 
E ([x — X(z)]z' }=0 
MFR) 是 x 在 z 上 的 正 交 投影 ， 并 记 x(z)-E[xiz] - 
车 将 x 看 做 被 估计 量 ，z 为 观测 量 ， 则 当 x 是 基于 z 的 线性 最 小 均 方 误差 估计 时 ， 


5 


恰好 具备 正 交 投影 的 上 述 三 个 条 件 ， 因 此 投影 是 存在 的 ， 反 过 来 可 以 证 明 ， 具 备 这 三 个 
条 件 的 x(z) 也 只 能 是 线性 最 小 均 方 误差 估计 ， 也 就 说 明 投 影 是 唯一 的 。 
根据 上 面 的 正 交 投影 定义 ， 可 得 到 以 下 几 个 重要 的 性 质 : 
(1) 设 x* 和 z 分 别 为 n, 维 和 nn, 随机 矢量 ， 则 x 在 z 上 的 投影 ， 唯 一 地 等 于 基于 2 的 
线性 最 小 均 方 误差 估计 ， 即 
Elxlz]=E[x]}+cov(x,z)[cov(z)] ` (z — Elz]) 
简单 证 明 如 下 : 
由 正 交 投影 条 件 (D 和 “2) 可 知 
E[&(z)]-AE[z] -5—E[x] 
于 是 有 
b=E[x]— AE[z] 
X(z)-Az*E[x] - AE[z]=E[x]+A(z — E[z]) 
再 根据 正文 投影 条 件 (3), (8 
E([(x — Elx]) — A(z — E[z])]z' )=E([(x — Elx]) - AG — Elz DK — Elz]+Elz))')=0 
对 上 式 进 行 求解 ， 得 
A-cov(x,z)[cov(z)] ` 
(2) Wx #lz 4) 5918 n, HER n, ERE, BA pxn, EEEE, MWM 
É(Bx|z)-BÉ(x|z) 
(3) x H n, EMINRE, alaa zh] ，z 为 n, 维 随机 矢量 ， zi 为 n,x(k 一 1) 
ERE, 
Ê (lzi FÊ- p Sz) (1-35) 
AP: xx - ÉGx|z,, 1) ; =z- É(z, I4 1) ° 
证 明 : 由 于 正 交 投 影 的 无 偏 性 ， 因 而 有 ElX]=0 ，E[z]=0 。 为 了 证 明 式 (1-35)， 首 先 令 
x"=E[xla 1]+ELXIZ,] 
且 根 据 性 质 (1)， 有 
x°=E[x|z, 1]+ELXZ: 1(E[z, yZ (1-36) 
现在 只 需 证 明 x* 是 x 在 zx 上 的 正 交 投影 , 为 此 只 需 根 据 正 交 投影 的 定义 , 验证 x° 
满足 正 交 投影 的 三 个 条 件 。 
QD x' 可 由 zx 线性 表示 ， 为 此 可 令 
Elxlz_ 1 ]=b +A (1-37) 
Elz, la, 1]=b,+A,z , (1-38) 
将 式 (1-37) 和 式 (1-38) RAR (1-35)， 并 令 
C-E[xz, HELZ ]) ' 
由 此 可 得 


x`=b +Az ,*C[z, - b, — 4,241] 
7(b, — Cb, )*(A, — CA, )z,, ,* Cz, 


=b+[4, - CA, c| 7 [nata 
Zk 


式 中 
b-b, -Cb,, A-[A, - CA,C] 
上 述 分 析 表 明 x° 是 ax 的 线性 函数 ， 亦 即 x* 可 由 z, 线性 表示 。 
© x 满足 无 偏 性 ， 即 E[x']-E[x] 。 
直接 对 式 (136) 取 期 望 ， 易 得 
E[x' JE (É[xlz, ,]] +E ET, £; ]} El, FEl] 
© x 满足 正 交 性 ， 即 E[(x — xz ]=0 , 


首先 有 
E(x[ÉGz Iz, ,)] )-0 
E(z,[£ (lay .)] )=0 
根据 上 述 两 式 可 得 
E (Sz, =E (42, +E( le 1)] )-E GG; ) 
E (£, zL)-E (£,[Z, +E la 1)] ) -E (2,21 ) 
现在 证 明正 交 性 : 


E[(x — x° Jz; ]-E([x - E(x- 2,4.) - Ez EB D) z z 
-E[Xz, ] - E[XZ; EZ, Zt D) Elz 24] 
=[E(&Z⁄,) E, )] - EZ, ) (EE, D) LE (I Zika) EEZ) 
-[0 E(xz; )] -[0 E(Xz; XE[z,; D E(Z,2; )]=0 
正 交 性 得 证 。 
上 述 证 明 过 程 表 明 ，x* 满足 正 交 投影 的 三 个 条 件 ， 是 x 在 ax 上 的 正 交 投影 ， 可 表 
示 为 
x' -É[xlz,, Êl HÊ] 
也 可 表示 为 
Elxlz F Ella, HEE EG, ZI )]. Z, 
2. 卡尔 受 滤 波 方程 
仍然 采用 式 〈1-7) MR 1-8) 的 系统 模型 ， 假 设 六 维 随机 矢量 x, 基于 观测 量 
Z 7 {a22 ) 的 线性 最 小 均 方 误差 估计 是 x) 在 ax 上 的 投影 ， 表 示 为 
$ 7Élx las] 
4 j=k 时 ， 可 得 滤波 估计 


A A ^ ^ ^ 
Xk = Xy =E[x,|z,, ]- E[x, |z, 1 .2,] 


由 正 交 投影 性 质 (3)， 得 
X, -É[x, Iz p [Xi -l Zip- J[E (Sa Sea W. Za 
式 中 
Xaca 一 Mk 一 Elx, Iz] 


£c 72, — Elz; Zi] 


A (1-39) 即 为 卡尔 曼 滤 波 方程 ， 下 面 讨 论 该 式 中 各 项 的 具体 计算 表达 式 。 


Élx, Iz Ffw- =Ê (Fx, tW- FX, , 
因为 Efwi_zx4]=0， 故 Épw, iz... ]-0. 
条 的 预测 误差 为 
Za =z 一 Elz, li- =k 一 Ê [Hx, +vi Zi] 
K. Sm É[Hx, liz- 7 Ht, , 
X O40) 又 可 表示 为 
Zrk-1 =k 一 É[z, lix- ]- Hx, +v, 一 Hx, | 7HX, 4 Ty, 
根据 上 式 即 可 求 得 
BI aa 1 ]-HP,, ,H ^ *R, 
E[X,y. ai ]- EX. (Hx,, 十 内 ) ]-B,, H " 
将 式 (1-40) ~A (1-43) RAR (1-39), 4 
X, = +K, G, 一 HX) 
式 中 
K,-P,, ,H j (HP,, H a +R, y 
滤波 误差 协 方差 矩阵 的 推导 与 1.2.1 节 相 类 似 ， 在 此 不 再 著述 。 


13 卡尔 曼 滤 波 的 扩展 


1.3.1 完整 形式 的 卡尔 曼 滤 波 


考虑 线性 动态 系统 
x, = Fx, + Tu + Gw, , 
z, = Hx, + Dv, 


(1-39) 


(1-40) 


(1-41) 


(1-42) 
(1-43) 


(1-44) 


(1-45) 


(1-46) 


AP: x, 是 nn, 维 状态 矢量 ，z š n, HENARES u, ARARE: F. TAHA 


CAERE; we 和 vw 是 相互 独立 的 高 斯 白 噪声 且 有 
E[w]=w, cov[w,]- Qô; 
E[v,]2 v, cov[v,]- Ró, 


(1-47) 
(1-48) 


假设 噪声 均值 交 、V 和 协 方差 Q 、R 均 为 已 知 量 ， 则 此 时 的 卡尔 曼 滤波 算法 为 


x, = Xua Ko, (1-49) 


D, = 不 一 Eee 一 DY (1-50) 
Tip = FX, + Tu, , + Gw (1-51) 
K, = P4, ,H'|HP,, H" + DRD']' (1-52) 
P = FP, ,F! - GQG' (1-53) 
P, - [I - K, H]P,, , (1-54) 


1.3.2 ”噪声 未 知 时 的 滤波 算法 
当 噪 声 统计 特性 市 、7 、@ . R 未 知 时 , 连同 状态 xo, x, x, 的 最 大 后 验 估计 ，w 、 
V 、Q 、R 与 Xj 可 通过 最 大 化 如 下 条 件 概率 密度 求 得 
J° = Plxox, wv ,QO,R|z. ] (1-55) 
AP: xo 和 zy 分别 表示 从 初始 时 刻 到 Kk 时刻 目标 的 状态 量 和 观测 量 ， 其 实质 是 通过 所 
有 时 刻 的 观测 对 以 上 五 个 参数 进行 估计 。 因 为 噪声 是 高 斯 白 噪 声 ， 所 以 该 条 件 概率 密度 
可 以 写成 解析 表达 式 ， 并 通过 求 对 数 、 求 导 运 算 ， 得 到 最 优 解 的 表达 式 为 


1 «&,. Š 

WD =T, -Fe Tu] (1-56) 
ja 

^ 1 £ A A 一 A ^ 一 人 了 
j=1 

A 1 k A 
j=1 

A 1 k A — ^ L1 

R, 12/5 - Hx, -v lz; - Hx, -v] sn 
j=1 


以 上 解 是 最 优 解 ， 因 为 式 中 的 估计 通过 个 时 刻 的 值 进行 平滑 处 理 得 到 ， 所 以 最 大 程度 
地 利用 了 观测 得 到 的 信息 。 但 是 在 实际 的 应 用 中 ， 这 也 将 意味 着 要 对 数据 进行 批 处 理 ， 
实时 性 不 强 。 为 此 ， 在 误差 能 容忍 范围 内 ， 用 滤波 估计 区 或 预测 估计 区 ij 代替 计算 复 
RRP y ， 可 求 得 次 优 MAP 估计 ， 即 


"oro 

Ñ, = 二 - F&,,-Tu,] (1-60) 
j=l 

a lé. = Sas - 425 z 

0 =€, - Få, -Tu - WX, - Få,- Tu,- W)" (1-61) 
j=l 

. l«& " 

$, T1246, - H£,1) (1-62) 
j=l 

. l« ^ ^ —\T 

R,-- 2, -HX y - v); - Hx, ,-v) (1-63) 
j=1 


可 以 证 明 ， 由 式 (1-600 一 式 〈1-63) Zi B pov vw RE 9 déc. BE QUE ñ E 
有 偏 的 ， 可 以 求 得 修正 后 O # R 的 次 优 无 偏 MAP 估计 为 


"EC. 

Ó, = pE; +P,- FP _F'] (1-64) 
J= 

& l4 

R, -32.b/5 - HP,, H`] (1-65) 
j=1 


以 上 结果 虽然 无 偏 ， 但 是 因为 有 累加 的 过 程 ， 计 算 依然 不 方便 ， 进 一 步 推导 可 得 次 优 无 
偏 MAP 估计 的 递 推 形式 为 


Wesi -— ò, tX, 7 FX, - Tu) (1-66) 
: E. lá 

CH = 十 K, Sra tb a ku * Pn FP,F') (1-67) 
r 1 à 

Wa = r,” +Z - Hx, ik) (1-68) 
á 1 

Rin = k IU + Denden + HP. nH ) (1-69) 


通过 以 上 公式 ， 利 用 卡尔 曼 滤 波 算法 ， 就 可 以 即时 估计 出 过 程 噪声 和 观测 噪声 的 参 
数 ， 实 现 自 适 应 滤波 。 


1.3.3 ” 带 未 知 参量 的 滤波 算法 


在 很 多 情况 下 ， 系 统 中 还 包含 一 未 知 的 参数 ， 与 一 般 的 系统 状态 (随机 矢量 ) 不 同 ， 
未 知 量 为 确定 量 ， 此 时 式 〈1-1) 和 式 〈1-2) 的 一 种 重要 扩展 形式 可 写 为 
x, = F(0)x,_, + w, (1-70) 
y, = H(0)x, + v, (1-71) 
对 于 这 类 系统 ， 在 完成 状态 量 x, 滤波 估计 的 同时 需要 得 到 参数 0 的 估计 ， 该 过 程 一 
般 称 为 系统 识别 (或 对 应 目标 跟踪 中 的 运动 模式 识别 )。 对 上 述 问题 的 求解 通常 有 两 种 方 
法 ， 一 种 是 将 状态 量 和 参量 分 开 来 考虑 ， 分 别 用 传统 的 卡尔 曼 滤 波 算法 来 处 理 〈 称 为 双 
重 滤波 ，Dual Filtering)， 且 两 者 交替 进行 ， 即 状态 估计 的 结果 用 于 参量 估计 ， 而 参量 估 
计 的 结果 反 过 来 又 为 状态 估计 所 用 ; 另 一 种 更 常用 的 方法 是 将 状态 量 和 参量 组 成 联合 状 
态 ， 再 采用 一 般 的 卡尔 曼 滤 波 算法 来 处 理 〈 称 为 联合 滤波 ，Joint Filtering)。 下 面 主要 针 
对 第 二 种 思路 进行 重点 介绍 。 
3⁄4 x, 5 0, 进行 组 合 得 到 扩展 状态 量 xz pL 0L] ， 并 将 确定 参量 & 进行 随机 化 


0,=0,_ +ç, (1-72) 
AP: ç, 为 零 均 值 白 高 斯 噪声 ， 方 差 为 & 。 值 得 注意 的 是 ， 上 述 随 机 化 过 程 是 必需 的 ， 
否则 就 无 法 得 到 0, 的 估计 结果 。 

在 上 述 假设 条 件 下 ， 式 (1-70) 和 式 (1-71) 可 进一步 写成 
-—" 4, | FO) 9| x, W, 

x, =F tme 0 MPG (1-73) 

X, 
y, = H° x; +v, =|[H(0,) NE (1-74) 


由 此 可 得 到 如 下 的 滤波 算法 流程 。 


Alg (1.1): Joint Estimation Kalman Filtering 
步骤 1: 初始 化 


.| 癌 | [ EG) _[covo) 0 

x ed š | 0 s| 
步骤 2， 时 间 更 新 

fa = Xa F (0, .) 0 | Xa 

"UU Haa 0  1,J8,, 


^ ^ T 
n e E P i J in ^| 


0 I, 0 I, 0 S, 
步骤 3: 量 测 更 新 
^ X, Šik- A A 
f= A = ^ K P H 0 0 E 1 
" H Pai n [ ky) Ji- 


P,=(1 — K,[H (Ôk) 012, 


-l 


K,-P,, lA Âp) OT (LH Âp) 0], HOw) 01 +R,) 
从 上 述 计算 过 程 可 以 看 出 ， 若 将 9 建 模 为 确定 性 常量 ， 即 9, = ，S,=0 ， 则 经 递 
EEAS Âu Âa ， 独 立 于 量 测 数 据 ， 从 而 无 法 完成 对 9 的 量 测 修正 和 估计 。 


134 ”卡尔 曼 滤 波 的 求 根 实现 

对 于 由 式 (1-7) 和 式 〈1-8) 所 描述 的 线性 、 高 斯 系统 ， 最 优 卡尔 曼 滤波 算法 中 逢 
阵 的 Riccati 方程 解 要 求 该 矩阵 为 对 称 的 半 正 定 方 阵 , 但 在 实际 应 用 中 , 受 计算 机 有 限 
字 长 等 因素 的 影响 ， 售 入 误差 的 存在 往往 会 导致 及 矩阵 为 非 对 称 、 非 正定 。 对 于 状态 矢 
量 ， 若 其 所 包含 的 某 些 元 素 在 量 级 上 相差 悬殊 ， 数 值 问题 就 尤为 突出 。 

为 了 说 明 这 一 问题 ， 假 定 * 阶 矩阵 P 具有 7 个 特征 值 0(i=1,2,…,n) ， 若 所 有 特征 值 


AE, MKP 为 可 逆 正 定 和 矩阵， 其 条 件数 定义 为 
6 (P) 
A(Py- o. (P) (1-75) 
24 A(P) > eo 则 称 和 矩阵 为 病态 和 矩阵 〈 条 件数 差 ) 或 奇异 矩阵 。 
在 卡尔 曼 滤波 算法 中 ， 误 差 协 方差 矩阵 P=E[(x -人 (= 一介 "]， 说 明 该 矩阵 为 正定 的 


或 者 等 效 说 该 矩阵 所 有 的 特征 值 均 大 于 零 。 但 若 立 的 某 些 元 素 的 估计 精度 要 比 其 他 元 素 
高 得 多 ， 例 如 
_ 10 0 
P p hr (1-76) 


X (1-76) 说 明 ，x[1j 的 标准 偏差 为 10”， 而 x[2] 的 标准 偏差 则 为 10” ， 这 种 现象 在 实际 
中 确实 存在 ， 这 是 因为 各 个 分 量 可 能 采用 了 不 同 的 单位 或 者 各 个 分 量具 有 不 同 的 可 观测 
性 。 显 然 ， 根 据 前 面 的 定义 ， 此 时 和 矩阵 的 条 件数 4(P 六 20 。 在 这 种 情况 下 ， 计 算 机 《处 
理 器 ) 很 可 能 将 其 视 为 奇异 矩阵 。 这 是 因为 ,假定 运算 过 程 中 采用 了 十 进 制定 点 处 理 器 ， 
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100 项 在 处 理 器 中 可 正确 表示 ， 而 10…" 项 则 被 视 为 0 来 对 待 。 由 此 可 见 ， 数 学 上 ， 和 矩阵 
P 是非 奇 异 的 ， 而 在 应 用 计算 时 ，P 则 是 奇异 的 。 
改善 数值 问题 的 一 条 有 效 途径 是 采用 平方 根 滤 波 算 法 ， 对 于 矩阵 己 ， 若 将 其 表示 为 
P-SS'-S'S 
则 S 称 为 P 的 平方 根 (Square Root) 矩阵 。 值 得 注意 的 是 上 述 定 义 式 与 P=S ”是 不 同 的 ， 
实际 处 理 中 由 了 得 到 S 的 解 不 是 唯一 的 ， 最 常用 的 求解 方法 之 一 是 采用 Cholesky 分 解 。 
通过 求 平方 根 处 理 , 滤波 算法 中 将 采用 和 矩阵 5S 而 非 P 进行 递 推 , 这样 做 的 好 处 在 于 ， 
根据 式 〈1-75) 的 定义 ， 此 时 
ovs (S). [o (P) 
Omin (S) \ Omin (P) 
即 4(S)=V4(P)=105, 说 明 求 根 滤波 能 够 提供 2 倍 于 标准 算法 的 精度 ,可 有 效 帮助 避免 数 
值 计 算 引 起 的 问题 。 
下 面 讨论 平方 根 卡 尔 曼 滤波 的 具体 实现 过 程 ， 在 开始 介绍 之 前 ， 首 先 给 出 另 一 个 重 
要 的 结论 。 设 Si=[covCx) 2 , L 为 下 三 角 和 矩阵 且 五 下 =EP ,/H'+R, ， 则 Si 为 矩阵 
S Sisa -[FS, , Qj; Loa EP Qul (1-77) 
的 平方 根 ， 且 
S,78,, 4I — Sip- H" Q4) Qa *R,) 再 See] (1-78) 
AP: 上 标 1/2 表 示 和 矩阵 的 任意 平方 根 ， Ri 为 R, 求 平 方 根 得 到 的 下 三 角 和 矩阵 ,辅助 矩阵 
Sua HS, 《k=1,2,… ) 分 别 为 协 方差 矩阵 Pu A P, 的 平方 根 矩 阵 ， 该 矩阵 既 不 一 定 是 
下 三 角 阵 也 未 必 是 正定 阵 。 式 〈1-78) 的 详细 证 明 见 附录 B. 
据 此 ， 可 得 到 求 根 形式 的 卡尔 曼 滤波 算法 流程 如 下 。 
Alg (1.2): Square Root Kalman Filtering 
步骤 1: 初始 化 
S, =L cov (x, )] ^ ， Xo=E[xo] 
步骤 2: for k=]1,2,… 
计算 平方 根 和 矩阵 Ske- LSU 
Su” [FS, , Qn L... UP Si Qi r 
L, =y HP, ,H "+R, 
S, =S k-i W ag Sh HE à (i P a +R; ) HS, ] 
步骤 3: for k-12,--- 


得 到 状态 量 的 滤波 估计 为 
X, 7 FX, ,*K,(y, - HFš,_,) 
K, 2S, iSi H Ç (L )'L 
从 整个 滤波 算法 流程 看 ， 求 道 操 作 仅 限于 对 三 角 和 矩阵 进行 〈 三 角 矩 阵 的 求 逆 相 对 简 
单 )， 且 这 些 三 角 和 矩阵 均 为 平方 根 矩阵 。 
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14 应 用 实例 


1.4.1 ”系统 噪声 自 适应 估计 
设 某 线性 系统 的 状态 演化 过 程 为 
x, = Fx, + Gw,_, 


AP: x,=[x, x, A k EAR AES: G ASW W A EBE; wA EE Jr hL FEW 


声 ， 且 
AIC Irn ur 
r-l | cr| |. cov[w, ]"q 


z, = Hx, * v, 
RP: H=[1 0] 为 观测 矩阵 ， 六 为 未 知 高 斯 观测 噪声 。 
假设 状态 初 值 xv=[2000 -10] ，7=0.25 q-1, E[v,]-0, R-var[v,]-20? . 3t— 
步 假 定 初始 状态 估计 及 协 方差 矩阵 分 别 为 名 =[2030 —15]', P,-diag([80? 10^] ;初始 观 
测 噪声 均值 及 方差 估计 分 别 为 各 =0 ， 姑 =25? ， 采 用 1.32 节 中 介绍 的 噪声 自 适应 卡尔 曼 
滤波 (AKF) 算法 对 系统 状态 进行 估计 ， 所 得 结果 如 图 1.1 一 图 1.3 所 示 。 


2200 


观测 方程 满足 


2000. 


1800 


1600 


True vs.estimated x, 





0 100 200 300 400 
Time index 


图 1.1 状态 估计 结果 


True vs.estimated E[v,] 


True vs.estimated var[v,] 





0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400 
Time index Time index 
图 1.2 ”观测 噪声 均值 估计 图 1.3 3352 £46 


由 图 1.1 一 图 1.3 中 的 结果 可 以 看 出 ， 利 用 AKF 可 以 得 到 系统 状态 和 噪声 特性 参数 
(均值 和 方差 ) 的 估计 ， 且 随 着 递 推 过 程 的 不 断 持续 ， 估 计 误 差 逐渐 减 小 并 趋 于 稳定 ， 说 
明了 AKF 算法 的 有 效 性 。 


1.4.2” 匀 加 速 机 动 目标 跟踪 
假设 二 维 平 面 中 的 目标 运动 方程 满足 
x, = Fx, +D u, + Gw, , 
式 中 : x,=[x, X, y, z] 29 k AU EUER u,=[a,a,Y 为 控制 项 ，F 状态 转移 矩阵 ; 
Tr 为 控制 输入 矩阵 ，C 为 噪声 输入 和 矩阵; wa 为 零 均 值 高 斯 过 程 噪 声 ， 且 


170 O 72/2 0 
0 1 0 0 T 0 
F= , T=G= : - 
0017 Ó Tn cov[w, ]-Q, 
0 0 0 1 0 T 
观测 方程 为 
z, = Hx, + v, 


AP: H 为 观测 矩阵 ，w 为 零 均值 高 斯 观测 噪声 ， 且 


adi 9 0 9 "U 
o 0 1 oj OPER 


对 跟踪 系统 而 言 ， 目 标的 状态 及 控制 输入 都 是 未 知 的 ， 此 时 ， 可 将 目标 状态 和 控制 
输入 组 成 扩展 状态 量 xe=[x; uL] ， 由 此 得 到 扩展 条 件 下 的 状态 方程 和 观测 方程 分 别 为 
x, =F" xi ,*G^w, 
y, = H^ x, tv, 


其 中 


0,., I, 
假设 目标 初始 状态 为 [20km - 200m/s 15km 100m/s]! ， 控 制 项 为 [6 -8 m/s? , 
T=0.5s ， 过 程 噪声 和 观测 噪声 协 方差 为 @,=diag([(2m/s*) (2m/s2)”]), R, =diag([(20m)° 
(20m)*]) ， 初 始 状 态 估 计 和 协 方差 估计 分 别 为 
&2=E[x#]=[20050 —210 15050 110 0 0]" 
P; —cov[x; ]-diag([200^ 50? 200? 50? 20? 20? ]) 
利用 联合 估计 卡尔 曼 滤 波 算法 对 目标 扩展 状态 进行 估计 ,所 得 结果 如 图 1.4 一 图 1.6 所 示 。 
由 图 1.4 一 图 1.6 中 的 结果 看 ， 利 用 状态 量 扩展 方法 并 结合 卡尔 曼 滤 波 ， 可 实现 对 目 
标 位 置 、 速 度 及 控制 输入 《恒定 加 速度 ) 的 有 效 估 计 ， 且 估计 误差 在 较 短 的 时 间 内 趋 于 
收敛 状态 。 另 外 ， 从 图 1.7 和 图 1.8 中 不 难看 出 ， 滤 波 器 收敛 后 (如 大 于 10s)， 控 制 项 
的 估 值 与 真 信 曲 线 几乎 完全 重合 ， 且 估计 误差 在 +lo 范围 之 内 (0,=| P: | (=5.6) )， 


说 明 结 果 是 稳定 的 。 
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图 1.7 x 轴 方 向 加 速度 估计 图 1.8 》 轴 方向 加 速度 估计 
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本 章 主要 介绍 非 线 性 滤波 的 原理 基础 。 首 先 给 出 了 贝 叶 斯 滤波 的 求解 通 式 ， 并 针对 





线性 、 高 斯 的 特殊 条 件 ， 重 点 阐述 了 最 优 解 的 形式 一 一 卡尔 曼 滤 波 原理 和 实现 方法 。 结 
合 实际 应 用 中 面临 的 几 类 常见 问题 ， 本 章 还 介绍 了 卡尔 曼 滤 波 算法 的 扩展 形式 ， 并 给 出 
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两 种 典型 应 用 一 一 噪声 在 线 自 适 应 估计 和 系统 控制 输入 估计 的 卡尔 曼 滤 波 应 用 实例 。 

应 该 指出 的 是 ， 卡 尔 曼 滤波 是 在 线性 、 高 斯 条 件 下 最 优 贝 叶 斯 滤波 的 特例 之 一 ， 对 
于 其 他 两 种 特例 ， 由 于 其 应 用 不 具 普 遍 性 ， 本 章 没有 作 更 多 的 探讨 ， 有 兴趣 的 读者 可 参 
阅 相 关 的 文献 ， 如 文献 [9]、 文 献 [11] 等 。 另 外 ， 线 性 、 高 斯 条 件 下 的 最 优 卡 尔 曼 滤波 原 
理 是 书 中 后 续 章 节 将 详细 阐述 的 各 种 〈 非 线性 、 高 斯 / 非 高 斯 条 件 下 ) 次 优 卡 尔 曼 滤波 原 
理 和 实现 方法 的 基础 ， 深 入 理解 其 推导 过 程 和 假设 前 提 ， 对 合理 扩展 和 正确 应 用 卡尔 曼 
型 滤波 方法 (在 卡尔 曼 滤波 框架 下 导出 的 滤波 方法 ) 具有 重要 的 意义 。 


第 2 章 ” 非 线 性 跟踪 应 用 与 性 能 评估 


由 于 客观 世界 中 非 线 性 问题 的 普遍 性 ， 使 得 非 线性 滤波 理论 和 方法 具有 极为 广泛 的 
用 途 ， 其 中 最 主要 的 应 用 领域 之 一 是 目标 跟踪 。 

在 非 线 性 目标 跟踪 中 ， 主 要 涉及 三 个 重要 问题 ， 即 模型 、 方 法 和 评估 。 在 这 三 个 问 
题 中 ， 模 型 为 基础 ， 既 包括 目标 的 运动 模型 又 包括 观测 模型 ， 其 中 运动 模型 的 选择 取决 
于 目标 的 类 型 及 运动 状态 ， 而 观测 模型 则 由 传感器 类 型 及 观测 量 决定 ， 方 法 为 核心 ， 它 
是 实现 各 种 条 件 下 目标 状态 滤波 估计 的 重要 手段 ;评估 为 补充 ， 主 要 用 于 评价 模型 和 方 
法 的 可 行 性 、 有 效 性 和 精确 度 等 。 

本 章 主 要 讨论 模型 和 评估 问题 ， 重 点 针对 雷达 /红外 目标 跟踪 应 用 ， 介 绍 常用 的 目标 
运动 模型 及 在 不 同 传感器 条 件 下 的 观测 模型 。 考 虑 到 传统 雷达 观测 的 特点 ， 专 门 介绍 了 
二 维和 三 维 观测 条 件 下 的 去 偏转 换 方 法 。 在 此 基础 上 ， 总 结 了 目标 跟踪 中 的 一 些 常 用 评 
估 指 标 。 由 于 目标 的 跟踪 滤波 算法 有 很 多 ， 且 各 种 算法 的 性 能 不 一 ， 在 这 种 条 件 下 ， 有 
必要 寻求 一 种 理论 上 最 优 的 性 能 作为 各 种 算法 的 评估 参考 标准 。 克 拉美 一 罗 限 是 不 依赖 
于 具体 算法 而 能 达到 的 状态 估计 误差 理论 下 限 ， 可 用 于 评价 各 种 算法 能 否 达到 最 优 及 在 
何 种 程度 上 接近 最 优 。 有 鉴于 此 ， 本 章 最 后 重点 阐述 了 目标 状态 及 混合 状态 两 种 条 件 下 
的 克拉 美 一 罗 限 的 分 析 和 计算 问题 。 非 线性 滤波 方法 作为 本 书 的 核心 部 分 将 在 后 续 的 章 
节 中 分 专题 详细 阐述。 


2.1” 非 线性 跟踪 系统 模型 


2.1.1 目标 运动 常用 模型 

1. 二 维 情 况 

1) 3157332 JJ 

考虑 直角 坐标 系 中 二 维 平面 内 的 目标 运动 ， 若 目标 状态 〈 包 括 位 置 和 速度 ) 为 
x,-[xxy y] (k), WAERO (CV) 运动 时 ， 其 状态 方程 可 表示 为 


F 0 
x, = Fox, i tw = 0 F Xg + W, (2-1) 
mw X f 
Q, =cov[w, ]=q E M (2-2) 


式 中 


IT TB Da 
F. = » = 
- 1l 2-7 4 


T 为 离散 采样 间隔。 
2) 近 匀 加 速 运动 


若 目 标 状态 包括 位 置 、 速 度 和 加 速度 ， 即 x, =[xxxXy y yE (k), ， 则 近 匀 加 速 (CA) 
运动 时 的 状态 方程 为 


F 0 
x, = Fea Xr tW, = Xe 十 W, 


0 F, (2-3) 
" _2|2 0 
Q, ^ cov[w, ]=q $ el (2-4) 
式 中 
DT TU 75/20 T*/8 T?/6 
F,= 0 1 T|,Q9-rTr/s r5 rn 
0 0 1! Tus TUS T 


3) 近 匀 转弯 运动 


类 似 地 ， 设 x -[xxy 苛 [(b ， 当 目标 做 转弯 率 〈 角 速度) 为 中 的 近 匀 转弯 〈CT) 
机 动 时 ， 其 状态 方程 可 表示 为 


l sin oT 0 cosoT -1 


e eo 
— Aaa 0 cosoT 0  -sinoT EM CS 
TOT ERE 0 1—cosoT 1 
@ @ 
0  sinoT 0 cosoT 
Q, =cov[w, ] 
2(@T —sinoT)  1—-cosoT 0 oT — sin oT 
O° o o 
1-cosoT T €T -sinoT 0 
- a o (2-6) 
Ex 0 eT —sinoT  2(oT —sinoT)  1-cosoT 
o» a» o» 
— oT 0 — T 
e e 
2. 三 维 情况 


1) 近 勾 速 运动 
考虑 直角 坐标 系 中 三 维 空间 中 的 目标 运动 ， 若 目标 状态 (包括 位 置 和 速度 ) 为 
x, =[xxypz2] (Kk) ， 则 当 目 标 做 近 勾 速 (CV) 运动 时 ， 其 状态 方程 可 表示 为 
F 0 0 
0 F 0 
0 0 F 


x, = Fox, tW, = Xia TW, (2-7) 








Q 0 0 
Q,-cov[w,]-q^ | 0 Q, 0 (2-8) 
0 0 Q, 
式 中 
qa PA TR 
a-| i 2-7: ig 


2) 近 匀 加 速 运动 
若 目标 状态 包括 位 置 、 速 度 和 加 速度 ， 即 x, =[xxxXyyyz2z7 (k), MEE 


《CA) 运动 时 的 状态 方程 为 





F, 0 0 
X=Fcaxetw=| 0 F, 1 (2-9) 
0 0 F, 
0, 0 0 
cov[w, ]=q’ | 0 Q, 0 | (2-10) 
0 0 Q 
式 中 
Dp T. 75/20 T'/8 T/6 
F,=|0 1 T|, @ =| 7T/8 T5 rn» 
00 1 Th TH OT 











3) 近 匀 转弯 运动 

三 维 条 件 下 的 匀 转 弯 模 型 较为 复杂 ， 一 种 简化 的 途径 是 将 目标 运动 约束 于 某 一 平面 
(水 平面 或 斜 平面 内， 此 时 可 得 到 和 斜 平面 中 的 近 勾 转弯 模型 。 假 定 x =[xxXXyypz 
z2]"(k)， 转 弯 率 的 大 小 为 a， 则 三 维 “ 空 间 中 ”的 平面 近 勾 转弯 运动 可 表示 为 








F 0 0 
x, = Ferx, tw = 0 F, 0 |x +W, (2-11) 
0 0 F, 
q; Q, 0 0 
cov[w]=| 0 4, 0 (2-12) 
0 0 q; Q, 
式 中 
1 (sinoTyo (1-cosoT) o 
F,= 0  cosoT (sin oT)/o 
0 -—osinoT cos oT 
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6@T —8sinoT-sin2oT 2sin'(oT/2) -2@T+4sin oT —sin2oT 





4o w 4a» 
_ 2sin* (w7/2) 2o7 —sin2oT sin? oT 
zr 3 2 
-20T +4sin oT -sin 2@T sin? oT 2oT+sin 20T 
40? 20° 4o 


需要 指出 的 是 ， 上 述 给 出 的 是 几 种 最 常用 的 模型 ， 在 目标 具有 复杂 机 动 的 条 件 下 ， 其 
相应 的 模型 也 变 得 复杂 。 针对 这 一 问题 , 感 兴趣 的 读者 可 进一步 参阅 文献 [12]、 文献 [13] 等 。 


2.4. ”传感器 观测 模型 


1， 点 目标 测量 模型 

如 图 2.1 和 图 2.2 所 示 为 二 维 平面 、 三 维 空间 条 件 下 的 目标 CT) 一 传感器 (O) XXI 
测 几 何 关系 ， 在 点 目标 假设 条 件 下 ， 不 同 的 传感器 可 得 到 不 同 的 观测 量 ， 如 典型 的 主动 
雷达 观测 可 包括 距离 、 方 位 /俯仰 及 径 向 速度 等 ， 被 动 雷达 、 红 外 传感器 一 般 仅 能 提供 方 
位 /俯仰 量 测 信息 。 下 面 分 二 维 、 三 维 两 种 情况 介绍 典型 的 点 目标 观测 模型 。 





图 2.1 二 维 目 标 一 传感器 相对 几何 图 2.2 三 维 目标 一 传感器 相对 几何 
1) 二 维 情况 
RHE: n=h, (x )+v, = Jx; +y2 um (2-13) 
方位 : 9, =h, (x, )*v, " -arctan(y, /x, )*v, k (2-14) 
š XX, Lp, y 
BERRE: Anh o), ESSE y, (2-15) 
Xy TY, 
2) 三 维 情况 
斜 距 : r.=h,(x,ytv,, x2 ty zo vy (2-16) 
方位 : 9,7h,(x,)*v, -arctan(y, /x, )*vo (2-17) 
俯仰 : 0, -h, (x, )*vo& -arctan(z, /VX +y; )+vox (2-18) 
I X,X, +y, p, +z, 
JEFE: Ach Ou) LÀ E. Ivy, (2-19) 
X, ty, tZk 


在 上 述 模型 中 : Ap) 表示 非 线性 观测 函数 ，? 为 观测 噪声 。 
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2， 点 扩散 模型 
1) 红外 图 像 
对 于 非 理想 点 目标 ， 红 外 观测 数据 表现 为 信号 强度 在 成 像 面 中 的 扩散 分 布 形式 上 。 
假设 红外 传感器 产生 的 每 一 帧 图 像 有 n, xm, 个 分 辩 单 元 〈 像 素 )， 每 一 个 分 辩 单 元 
(iJ) Gh, nj 71, m) E— A JEDER A. x A, , 则 大 时 刻 传感器 获取 的 mxmo 个 
强度 观测 数据 可 记 为 
s= [Db ng =l m] (2-20) 
式 中 : z4"” 表 示 k 时 刻 分 辩 单 元 (i,j 的 强度 观测 ， 具 体形 式 为 
z6) = pm vw? H, 
: ve H, 
式 中 ; hi (x, ) = 一 一 二 上 UA = av- — cni ! f, 为 信号 强度 ; vé? 3528 Gi, j) 
个 分 辨 单元 的 测量 噪声 ， 且 | 和 HH, 代表 目标 信号 的 存在 性 假设 ，HH 表示 有 目标 ，Ho 表 
示 无 目标 〈 若 无 特殊 说 明 ， 后 文中 均 采 用 相同 的 表示 方法 )。 
2) 雷达 图 像 
对 雷达 图 像 的 建 模 可 借鉴 文献 [13]、 文 献 [16] 中 的 方法 ， 以 回 波 信 号 的 距离 一 多 普 勒 
一 方位 图 作为 雷达 原始 观测 数据 。 限 定 感 兴 趣 的 传感器 观测 区 域 ， 以 传感器 所 在 位 置 为 
坐标 原点 ， 距 离 范围 为 [uin,mmx] ， 分 为 N, 个 距离 单元 ;多 普 勒 速 度 范 围 为 [guin,dus] ， 
分 为 Nj 个 多 普 勒 单元 ; 方位 范围 为 [bwio,bwax] ， 分 为 Ni 个 方位 单元 。 雷达 观测 数 据 为 散 
射 能 有 量 ，k 时 刻 一 帧 图 像 HE N, x N, x ,个 数据 ， 记 为 


(2-21) 


Zr = [fn "d =1,.…,N.,; = l,e, N ;l =1,--,N,| (2-22) 
AP: z 表示 分 辩 单 元 心力 0) 的 能 量 观测 ， 定 义 为 
zD - [zt (2-23) 
式 中 ，z4 思 表示 目标 的 复 幅 度 〈 复 散射 强度 ) 观测 数据 ， 可 表示 为 
T A hi) (x ) + yin H 
rr -| Vs "m , H, (2-24) 
式 中 :人 水 为 目标 的 复 幅 度 ; AUD x.) 为 分 辩 单 元 (i,j,?) 的 散射 形式 ， 可 进一步 定义 为 
A, = Áe , ó, e (0,2n) (2-25) 
Kie y P SAT. _ (b b) ) 
h, co- 吧 | 2r fa 2d, 2b, (2-26) 


R Q-26) 可 认为 是 雷达 图 像 上 目标 信号 强度 分 布 的 点 扩散 函数 ，r. 、d. 、b. 为 与 距离 、 
or 分 别 由 带宽 、 积累 时 间 以 及 波 东 宽度 决定 。/, 、is 和 
为 损耗 系数 ， d, I b, 分别 为 时 刻 目 标的 真实 距离 、 方 位 角 和 多 普 勒 ， 其 计算 公 
epi rs 15)rBf0 h, O) n ho Go.) É h,(x,) 所 示 。 vk” 为 分 辨 单元 (i,j,?) 
的 观测 噪声 ， 噪 声 在 全 空间 内 独立 同 分 布 ， 简 记 为 w ， 定 义 为 复 高 斯 形式 ， 即 
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W = Vp, + jVg, (2-27) 
XP: vL vo, 25379 0 均值， 方差 为 er 的 高 斯 白 噪声 。 
综 上 ， 可 得 分 辨 单元 (i,j,?) 的 能 量 观测 为 





T m | 4, CP Qe, ) + Vp, t lvo, | H, 
Wt = um = : (2-28) 
m +jvox | H, 
XX (2-28) 所 描述 的 观测 满足 指数 分 布 
1 1 
pG”) |x,) = 人 qu (2-29) 
其 中 参数 up) 的 定义 如 下 
T e 0201 
[A0 oy «2e H, 
20° H, 
(i, j-l) 2 
[7 (4) +20 aa 
20 
AP: PRÉPA AE. APP (x ) 定义 为 
M^ G) - (KPE) 
= ex BW, (d,- 4, _ (h - by, (2-31) 
MaN p r d b 
š d, b, 


3. 距离 扩展 模型 

距离 扩展 模型 主要 是 指 在 高 分 辨 条 件 下 ， 雷 达 目 标 强 散射 点 在 径 向 上 的 扩展 ， 根 据 
不 同 的 目标 外 形 ， 可 分 为 以 下 几 种 扩展 模型 。 

1) 线形 模型 

线形 模型 "如 图 23 所 示 ， 这 种 模型 主要 适用 于 长 、 宽 比例 极 不 协调 的 目标 〈 即 
在 某 一 维度 上 的 扩展 要 远 远大 于 另 一 维度 )。 设 4 为 目标 的 真实 长 度 ， 则 其 在 雷达 视线 
LOS ( 径 向 ) 上 的 长 度 可 表示 为 
L =£ lcos d, | + v, 7h(x, )*v, =£ bu : ZA 


pu n, (2-32) 
4x ey E +j2 





图 2.3 线形 观测 模型 示意 图 
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AFP: 办 为 传感器 视线 方向 与 目标 速度 方向 的 夹 角 《〈 下 同 ， 该 角度 有 时 可 等 效 为 方向 
角 025)， 六 为 径 向 长 度 观 测 误差 。 

2) 椭圆 模型 

如 图 2.4 所 示 ， 假 设 椭圆 目标 的 长 、 短 轴 分 别 为 4 和 4b，4 为 沿 主轴 方向 的 长 度 ， 则 
其 在 径 向 上 的 投影 可 表示 为 中 

=4 feos? ó, +(b/a)°sin2@, + v, =h(x, )*v, (2-33) 

AP: é BJ XO EK (2-32) 中 的 相同 ， 且 
XX, 十 其 其 


v» (2-34) 
J + YEAR YR 


é, = arccos 





图 2.4 椭圆 扩展 模型 示意 图 


3) 矩形 模型 


如 图 2.5 所 示 , 假设 目标 呈 方 形 ,长 度 为 a、 宽度 为 b， 其 运动 速度 方向 与 雷达 视线 
方向 为 生 ， 则 其 在 径 向 上 的 投影 长 度 为 
I, -acosó, + bsin é, ^v, =h(x, )+v, (2-35) 





图 2.5 矩形 扩展 模型 示意 图 


2.1.3 ”雷达 量 测 转换 


在 雷达 跟踪 系统 中 ， 为 方便 描述 ， 目 标 运 动 通常 在 直角 坐标 系 中 进行 建 模 ， 而 雷达 
量 测 一 般 却 是 在 极 坐标 〈 二 维 ) 或 球 坐 标 〈 三 维 ) 系 中 得 到 的 。 这 样 ， 雷 达 目 标 跟 踪 就 
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是 一 个 非 线性 问题 ， 需 要 采用 相应 的 非 线性 滤波 算法 来 实现 。 在 后 续 的 章节 中 将 会 提 到 ， 
在 绝 大 多 数 情况 下 ， 非 线性 滤波 算法 都 是 次 优 算法 。 若 能 对 非 线性 量 测 方程 进行 线性 化 
转换 ， 则 能 在 最 优 线性 卡尔 曼 滤 波 的 框架 下 进行 跟踪 。 去 偏转 换 量 测 卡尔 曼 滤 波 
(Converted Measurement Kalman Filtering, CMKF) 3 就 是 这 样 一 类 算法 ， 先 将 雷达 量 
测 通过 坐标 变换 表示 为 直角 坐标 系 中 量 测 的 伪 线 性 形式 ， 然 后 估计 转换 测量 误差 的 前 两 
阶 矩 ， 最 后 通过 线性 卡尔 曼 滤 波 来 处 理 。 
1. 二 维 去 偏 量 测 转 换 
在 极 坐 标 系 中 ， 相 对 于 目标 的 真实 斜 距 > 和 方位 角 9， 和 雷达 观测 得 到 的 斜 距 和 方 
位 角 On 可 以 定义 为 
]j 一 /十 产 
| (2-36) 


9,79tó 
假定 斜 距 量 测 误差 X 和 方位 角 量 测 误差 89 为 相互 独立 且 均 值 为 零 的 高 斯 噪声 ， 方 差 
分 别 为 o2 和 ez ， 且 有 


js cost@,) (2-37) 
Ym "n sin (Øp) 
则 式 (2-37) 可 进一步 写成 
+F D)-x^X 
b Li alid E (2-38) 
y,7(r*P)sin(p*g)-y*5 
TEx-rcos(g). y-rsin(g) RAR (2-38) 并 适当 变形 ， 得 
X=rcos@(cos@ -1)-TFsin @sin ø -r sin psin @+Fcosgpcos@ 
"A " » "m uw " (2-39) 
y-rsin ø( cos ø — 1)? cos psin Ø+r cos sin @+F sin ø cos o 


由 此 可 以 看 出 ， 误 差 地 、 字 依赖 于 目标 真实 的 距离 、 方 位 及 其 对 应 的 观测 误差 ， 且 
彼此 之 间 具 有 相关 性 。 在 量 测 噪声 为 零 均值 高 斯 噪声 〈 即 5B~N (5;0,0p)〉 假设 条 件 下 ， 
由 于 sin9 为 奇 函数 ， 易 得 

E[sin (@)]=0 
I 


-2 
„m f^ . @ T 
cos 2]- cos exp( - — 
E|cosg]- | cose D. o NP 
Too 1 p 
=Re cos Ø+jsin @ exp| —— do 
[ _ (cos@+jsin g) | 
] pe zm 4 LL. 
-Re| —— ex -— 
E |. xi 20; je 


z 
w y 1 |1(15 ol. 
zu ale = Ë 23 网 
RP: Re) 表示 取 复数 的 实 部 ，j 为 虚数 单位 (下 同 )。 
= 9 _ p 
4s V20, T7’ 代入 上 式 ， 得 
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2 
2 
o 
[e di 


E[cos? ë]Ü(1+e ^^* y2 


Elsin? ë]E(1 — e°” y2 
E[sin @cos @]=0 


Bleospl-exp| -22 | J” Lap(- ya 
< Jr 


同 理 可 得 


4e-[x y] ， 则 有 
HK=E[elr,P]=[A 4,1 


R-cov[elr.o] Ps "d 
=cov[elr,O]= 
R, R, 
由 此 可 得 
p=rcosg(e ^*^ —1) (2-40) 
-— 
L,-rsing(e ”一 ]) (2-41) 


R. = var[xir, gjere” [cos g(cosh(o; ) — 1)*sin^ psinh (c; )] 


(2-42) 
tore ^* [cos  pcosh (o; )*sin^gsinh (o;)] 
" r — 2 PESE. 2 2 ci 2 
R,,- var[y]r,g]-r e “[sin g(cosh(o;) - 1)*cos @sinh(o;)] — 
tole ^* [sing cosh (c; )*cos^gsinh (o;)] 
~ - . -202 c2 
R,,-R,, -cov[X,)|r,g]-singcosge '^*[o; +r (1- e^] (2-44) 


由 式 (2-40) ~ (2-44) 可 以 看 出 ， 量 测 转换 的 真实 偏差 及 其 协 方差 依赖 于 目标 
真实 的 距离 和 方位 。 但 在 实际 应 用 中 ， 目 标的 真实 距离 和 方位 信息 是 未 知 的 。 为 此 ， 在 
具体 应 用 时 , 可 在 量 测 值 mOn) 已 知 的 条 件 下 对 上 述 的 真实 均值 和 协 方差 矩阵 求 数学 期 
望 ， 得 到 平均 真实 偏差 和 平均 真实 协 方差 阵 ， 即 


Hwe Er, Pn J^ Ue e Havey T (2-45) 
M ust r s 
综合 利用 式 (2-36), R (2-45) MA (2-46, 4% 


-02/2 ) 


Rove =E[R|r, Pm 2l (2-46) 


Havex Tmn COS Pp (e> -e 


Havey m Sing, (e EE e 7 ) 
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=e??? (cos? @, [cosh(26; ) — cosh(o; )]*sin^ gp, [sinh (26; ) — sinh(c; )]) 


+o2e ?"* (cos°@, [2cosh (26; ) — cosh(c; )]*sin^g, [2sinh (26; ) -sinh (c; )] 


2 


Rony Tae op (sin? 9, [cosh(26; ) — cosh(o; )]*cos"g, [sinh (26; ) — sinh (c; )]) 


tole ?"* (sin^g, [2cosh (26; ) - cosh (o; )]*cos' o, [2sinh (2c; ) -sinh (c; )]) 


R... =R,,. y =sing,cosg,e “[o2+(2+o2)(1- e?)] 

需要 指出 的 是 ， 按 上 述 几 式 求 得 的 平均 协 方差 阵 要 大 于 真实 协 方差 阵 ， 这 是 由 于 以 
量 测 值 为 条 件 求 数学 期 望 引 入 了 附加 的 误差 。 另 外 ， 只 有 距离 较 远 、 方 位 误差 较 大 的 情 
况 下 ， 位 置 误差 和 上 面 提 到 的 协 方差 阵 的 增 大 才 会 显著 。 

由 此 可 知 ， 式 (2-37) 中 的 量 测 转换 可 修正 为 

_| Xem |_| Im COSPm | | se, 
" Jem | [msinw | | Lave 
此 时 ， 观 测 方程 可 建 模 为 
z. Hx, +v, 

AP: H 为 线性 观测 矩阵 ，v 为 等 效 零 均值 量 测 噪声 且 E[v,v, ] = Ruwer 。 

为 了 验证 上 述 去 偏转 换 方 法 ， 假 定 目标 真实 距离 和 方位 角 分 别 为 r-10km, @=45°, 
对 应 的 观测 精度 为 o,=100m ，o。,=1”。 通 过 蒙特 卡 罗 方 法 产生 500000 个 样本 并 根据 
A (2-37) 计算 得 到 目标 的 位 置 均值 (Xj) 与 目标 真实 位 置 xy) 的 偏差 及 相应 的 协 方差 分 

20230 -10229 PN SER 
PS C-L061,-079 和 | ^05 Mee 而 通过 式 (2-40) ~A (2-44) 计算 得 到 这 
20229 -—10227 

RREIZ EREA (10779, -10777 和 | P 50250 
方法 得 到 的 结果 非常 接近 ， 说 明了 去 偏转 换 方法 的 正确 性 。 

通过 上 述 分 析 可 知 ， 对 于 距离 一 方位 观测 ， 虽 然 观测 模型 是 非 线 性 的 ， 但 经 去 偏转 
换 后 可 变 为 线性 观测 模型 ， 进 而 可 以 采用 前 文 介绍 的 线性 卡尔 曼 滤波 算法 进行 处 理 。 

2. 三 维 去 偏 量 测 转 换 

在 球 坐 标 中 ， 相 对 于 目标 的 真实 距离 r 、 方 位 ?和 俯仰 9， 和 雷达 观测 得 到 的 斜 距 , 、 
Jj fü o, 和 俯仰 角 , 可 表示 为 


| 由 此 可 见 ， 两 种 


r,=r+F 
9,-9*6 (2-47) 
Ë =0+ő 
式 中 ,假定 量 测 误差 、j 及 6 为 零 均值 高 斯 噪声 且 相 互 独 立 , 其 方差 分 别 为 cz o 和 。 
采用 与 二 维 情况 类 似 的 方法 ， 可 得 到 球 坐 标 系 与 直角 坐标 系 下 的 转换 关系 为 
x, 7(r*F)cos(979)cos(0--8)-x*X 
y, "(r*?)sin (g+@)cos(0+0)=v+y (2-48) 
z, -(r^?)sin(0--0)-z4Z 
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式 中 
x-rcos(g)cos(0), y-rsin(g)cos(0), z=rsin(0) 


Sez pz] ， 则 有 
u-Eleir,o.0-Ua, 4, 4.] 


x R, K 
R-cov[e|r,o,0]-| R, Ry R- 
R. KR R. 


利用 独立 性 条 件 及 前 文 的 结论 ， 可 导出 如 下 结果 : 
#,=rcosg@cos oe?” e 9$ 1) 
44, 7rsingcos &(e ^e 99? -1) 


LL, —rsin G(e "9? —1) 


— WX 2 2 2 -02 -02 

R „= var [är p, 0] [r (a.a,, — cos“ pcos 0)*o,a,a, ]e *e ^? 
E. 

R,,- var[y|r,ə,0]=[r”(a,a,, -sin pcos?’ 0)*o; aya, ]e “° e ^ 


R, - var[Zir, o, 0]-[r" (a, ^ sin? 9)*o7a, je 


R „=R =cov[%jlr,ə,0]=[r (a, — e^ cos20)+ Ora, je?* e? singcosg 
R,,-R, -cov[£,zr,o,0]-[r^ (1 — e yro2 Je 7^ e? cos gsin Ó cos Ó 
R,-R, -cov[j.zir,o,]-[r^ (1 — e? )*o? je 7^ e? sin p cos sin 0 
其 中 

a,=sin2 gsinh a +cos pcosh c, 

a, sin? gcosh o; +cos? psinh o; 

a, -sin? Ocosh o; cos? @sinh o2 

a, 7sin" sinh o; cos? 0 cosh o3 


与 二 维 情况 类 似 ， 求 解 上 述 偏差 要 求 目标 真实 距离 、 方 位 和 俯仰 信息 已 知 。 在 这 些 
信息 未 知 的 条 件 下 ， 同 样 需要 利用 量 测 结果 对 误差 均值 和 协 方差 求 期 望 ， 即 


Have =E[lulr, Dm , 0, PULO Havey Havez P ( 2-49 ) 
r -— ) Ke. 
Re =E[R/r, Dm , 0, ]- Mio M oir Nd ( 2-50 ) 


Mun Sem Ras 
利用 式 (2-47) 对 式 (2-49) MA (2-50) 中 的 各 式 进 行 展开 ， 并 利用 三 角 关 系 式 ， 得 
Havex TICOSQ@,,COS 0, (e 7* e 7? — e 2 e 79?) 


2 2 2 Li 
k ° -Op -ap _ -op/2 -a$/2 
Havey r,sing,cos0,(e *e 6'*.- 


2 2 
E š -j -a$/2 
Ke n,sinB,(e '* -e79*) 
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_ 2 2 -2o2 -202 
"E im (bb, -aa )* c, (2b,b,, 一 aa je "e cà 
-202 _202 
R [72(b,b,, — aya, ) 0? Qb,b,, —a,a, )]e ”eo 
Rowa -lr?(b, — 2,)*07 (2b, - a,)je ^"* 
2 c2 2 cl. -4o -202 . 
Ras Rmo U Lü ae *)*o, (2b, -a etje *e' ^* sing, cos, 
2 
R, a =R,  =[r2 (0-67 yro2 (2 - e^? )]e “e 4° cosp, sin, cos@, 
2 
Roey =R, ,=[r2(1— e% yro2 (2 — e^? )Je “e ° sin o, sing, cosh, 
其 中 
a,=sin? o, sinh c; +cos? @, cosh c; 
a, =sin? @, cosh o; +cos° p, sinh o; 
a, sin? 0, cosh o; cos? 0, sinh o7 
a,, 7sin? 6, sinh as+cos 0, cosh o; 
b, =sin? g, sinh 26; * cos" g, cosh 20; 
b, -sin 9, cosh 2c; * cos! g, sinh 20; 
b, sin? 0, cosh 207 cos? 0, sinh 202 
b,, 7sin" 6, sinh 205 cos? 0, cosh 20; 


由 此 可 得 式 〈2-48) 修正 后 的 转换 量 测 公式 为 


Xom | | Fn COS Pp cosO， fas 
k=] Jan |^| 7, Sing, cosÓ, |— Peau 
Lo r, sin @, Have 
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2.2.1 通用 误差 评估 指标 
1. 均 方 根 误差 ( RMSE ) 


假设 第 m 次 实验 中 状态 量 x, e R”: 的 滤波 估计 为 疡 WU M 次 独立 实验 得 到 的 x, 估 
计 的 均 方 根 误差 可 表示 为 


RMSE, [n]- [Yes [n] - £g[n) ,n-12,--.,n, (2-51) 
m=] 


AP: x[n] 表 示 状 态 矢量 x 的 第 n 维 。 
2. 归 一 化 估计 方差 NEES ) 
W k MEA x, 的 滤波 估计 误差 为 总 =xi 一 总 ， 其 协 方差 矩阵 为 县 ， 则 归 一 化 估计 方差 
可 表示 为 
&, 7X, (P,) ! x, (2-52) 
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M 
在 线性 、 高 斯 条 件 下 , 上 服从 自由 度 为 几 的 卡 方 分 布 ( ), KE Ele, ] o > ern 


该 指标 常用 于 评估 滤波 估计 结果 是 否 具 有 一 致 性 。 
3， 归 一 化 平均 估计 误差 (NMEE ) 


假设 在 M 次 实验 中 第 普 次 的 估计 误差 为 区 ， 则 归 一 化 平均 估计 误差 可 表示 为 
u,[n]- xij. n-1,2,---,n, (2-53) 


M ma 


ERBRF. mn 服从 零 均值 、 方 差 为 1/M 的 高 斯 分 布 ， 该 指标 常用 于 测试 状态 
矢量 中 单 分 量 的 估计 一 致 性 。 


2.2.2 ”滤波 误差 理论 下 限 
1. 目标 状态 估计 误差 下 限 
现实 中 用 于 非 线 性 滤波 的 方法 可 能 多 种 多 样 ， 尽 管 通过 比较 研究 的 手段 可 以 评估 各 
种 方法 的 优 劣 ， 但 车 没有 一 个 理论 上 的 最 优 误差 限 ， 很 难 评价 一 种 方法 是 否 还 有 改进 的 
余地 或 在 多 大 程度 上 接近 最 优 。 由 此 可 见 ， 对 理论 误差 限 的 分 析 具 有 重要 的 意义 ®**9。 
但 需要 指出 的 是 ， 与 传统 参数 估计 的 误差 下 限 ( 称 为 克拉 美 一 罗 下 限 ，CRLB) 不 同 ， 
由 于 系统 为 动态 模型 ( 含 过 程 噪声 ), 该 条 件 下 得 到 的 状态 估计 误差 限 通常 称 为 后 验 克 拉 
美 一 罗 限 (PCRB)。 下 面 将 分 析 PCRB 的 计算 问题 ， 为 此 ， 重 写 式 〈1-1) MA (1-2) 
描述 的 非 线 性 系统 方程 为 
= f(x, _)tw, (2-54) 
y, = h(x,)*v, (2-55) 
其 中 ， 各 变量 所 代表 的 含义 与 前 文 所 述 相同 。 
WE xu = (x) A y, = 人 pn} 兴 为 到 丰 时 刻 为 止 的 累积 状态 序列 和 观测 序列 ， 则 在 任 
一 时 刻 k, Xor 和 yy 的 联合 概率 分 布 为 


k k 
Pr = PXo4, Vix) = PO JT p(y; | x, JT p(x; |x,4) (2-56) 
i=l jal 
idJGx,) CEKA (nk) x (n,k) ) 为 上 式 对 xo 的 Fisher 信息 矩阵 (FIM), J, (#E 
数 为 n, xn) AI DT WEFR W 即 为 状态 x 的 估计 误差 下 限 。 
若 将 Xo:k 分 为 两 部 分 即 Xo.k = [x2,.. 1 xl , 则 相应 地 
A, B, ]- hits jp) ECAZR "s 


J(xa)= E w 
C, E{- gom 'Innpj ECA ln p} 


T 
式 中 ， 站 和 人 分 别 为 一 阶 和 一 阶 偏 导 算 子 ， uv. ,As = ViVT。 


0a, 0m, Oa, 
根据 分 块 矩 阵 求 逆 定 理 ， 得 
J, = C, - BL A, B, (2-58) 
HET A, ERE J, 的 计算 涉及 A, 的 求 逆 。 很 明显 ， 随 着 的 不 断 增加 ， 其 运算 过 程 
极为 复杂 。 为 此 , 需要 导出 递 推 计算 公式 。 注 意 到 在 kH 时 刻 , 有 Xora m [xoa X; Xia] > 
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于 是 有 
Prai = PX oks orn) = P(Xok Joko Xen Jg) 
= Ph Xox» Yor) Pa | Xox» Joa) PU? | Xi Xor Yox) (2-59) 
= Ph Xos Vox) PG | XE) Pen | X4) 
E rA Inpin} E AS Ip, E Ea In Pisi} 
J(xo a a)=| EAR n pa} E(-A2Inp,) Ef{-Ax" I ppn} 
E(-Aj"Inp,,) ECA# npin} Et{-Ax* hn pu} 


bes iis "n (2-60) 
A B, 0 
=| B; C,*D, | D 
0 HD |D 
式 中 

D, = E{-A% In p(x,,, |x, )) (2-61) 
De = E(-Az" ln p(xin |x,)) (2-62) 
D; -[Dj] -E(-AZ In p(x,, |x,)) (2-63) 
Dj =E CAE In p(Xin |x,))* ELAR In P(O | x44)) (2-64) 


[]38, FIM FE Joa HEF Aon) 右 下 角 位 置 维 数 为 n, x n, 的 块 矩 阵 ， 对 照 
X (2-58)， 此 时 有 
-i 
J, =D? -[0 aF G is M = DP? - D? (J, + D)" D? (2-65) 
xX (2-65) 即 为 一 般 非 线性 系统 滤波 问题 所 对 应 的 信息 矩阵 的 递 推 关 系 式 ， 由 此 可 求 得 
状态 估计 的 后 验 PCRB 满足 P > J;. 
下 面具 体 分 析 PCRB 的 计算 问题 ， 主 要 分 两 种 重要 情况 : 
1) 高 斯 噪声 情况 
在 高 斯 噪声 假设 条 件 下 ， 可 设 w, =N 09,:0,9,) .. v, 7A (v,:0,R,) ， 初 始 状态 分 布 为 
x97 RR). 此 时 有 
p(xin | XEN (ins (x,),Q,) 
Pa | Xi )=N (zn ;h(x,.,),R,.,) 
于 是 有 
De = E[V,, f! (,)Q; Vy, f(x,)] 
Dr =—E[V., fT x,)]g;' =[D T 
De =Q; +E[V,, h Qa) Ris hn) 
上 述 几 式 中 都 存在 求 期 望 运 算 ， 实 际 应 用 时 常常 用 蒙特 卡 罗 方 法 进行 近似 。 假 设 进 
行 M 次 蒙特 卡 罗 实 验 ，{xr}x-o 表 示 第 m(m=1,2,…,M) 次 实验 中 状态 轨迹 的 样本 ， 则 


1 & =: 
Dj - 72 Va 0 V, f), (2-66) 
m=] 
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,=[Dr T (2-67) 


Xk xk 





M 
D? =- Y v, fe) 





a, 1 P 
D; =Q, E M 2, Fai h' (X, )R,aV s. h(x,,i) (2-68) 
m=1 


Xk+l =x + 
2) 线性 、 高 斯 条 件 
进一步 假设 式 (2-54) WMA (2-55) 所 描述 的 是 线性 系统 ， 即 f(x, ))-F,x,,, 
h(x,)-H,x, ， 此 时 根据 式 (2-61) 一 式 (2-65), f 
J, a =HiaR; Hnt -OG F, +F; O; F,) FQ, 
4 P =J; 并 利用 和 矩阵 引 理 ， 得 
P; a = (0, +F,P,F, ) *H, RH, 
-P *H a 
由 此 可 见 ， 该 式 正 是 卡尔 曼 滤 波 算法 中 的 协 方差 矩阵 表达 式 〈 以 道 矩 阵 的 形式 给 出 ， 
对 应 文献 [10] 中 的 信息 滤波 (Information Filtering) 公式 。 这 也 正好 说 明 线性 、 高 斯 条 件 下 


的 卡尔 曼 滤 波 是 最 优 估 计 ， 其 估计 误差 协 方差 矩阵 正好 对 应 理想 条 件 下 的 理论 误差 下 限 。 
2. 混合 状态 估计 误差 下 限 


考虑 更 一 般 情 况 下 的 误差 限 计 算 问 题 ， 即 状态 中 包含 一 未 知 的 常数 参量 9 ， 这 一 问 
题 在 实际 应 用 中 具有 普遍 性 〈 如 匀 转 弯 目 标的 转弯 率 、 匀 加 速 目 标的 加 速度 以 及 扩展 目 
标的 尺度 大 小 等 )。 此 时 ， 系 统 方程 可 记 为 
x, = f(x, ,,0)*w, (2-69) 
y, = h(x,,0)*v, (2-70) 
KHAIA (2-56) 类 似 的 方法 ， 在 任 一 时 刻 k, xx. yu 550 的 联合 概率 分 布 为 
Pr = p(Xo4, 14,0) = PO. Por) PO' ox ) p(0) 


类 Fo Xo l8 
k k (2-71) 
=p(x,)p(0)| [ 2G; lx O| [p(x 1x 1,0) 
il ja 
对 式 (2-71) 取 对 数 ， 得 
In p(x,, , p14,0) = In p(y,, or) pO Yor) PO) 
Jk Xo: X9: l9 
k (2-72) 


k 
-Inp(x,)*-In p(0)+ 5" p(y; | x, P2293 p(x; |x,4,0) 


i-0 
定义 
Kia = E(-V , [Va In p(xin |x,,0)] } 
K^. = E{-V [Ve ln p% 1x,,0)] ) 
Kin = E(-V,[V, In p(x,,, | x,,0)] } 
Li = E{-V „ [V ln p(z; | x,,0)] ) 
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L = E(-V,[V, In p(z; | x,0)] ) 

A, = E(-V,[V, In p(0)] ) 

A,7A, I5 I Ik 

i=l 

G,=L *K? +K% 
AP: i=0,1,-.k, D'ET, K AKT., Kñ [KA : ERK" Ko. D 维 数 大 
小 为 mxn ; 5gBk KA... D. A, 的 维 数 为 mxm， 当 i=0 时 对 应 的 K9" 3) mx n EER. 

此 时 ， 对 应 联合 状态 量 (xo, 0) 的 FIM 为 
Jy St V dus eV xo n In p(x», Zi ,)1) 


J K IG | (2-73) 


f oH — HAT 
G" LK? LA, 





式 中 
G'=[G, G, :-- G,] 
于 是 , 滤波 估计 的 误差 下 限 C, 为 ,大 右 下 部 分 大 小 为 (n+m)x (mm) 所 对 应 的 块 矩阵 
(具体 推导 见 文献 [25])， 且 Cr 中 的 块 元 素 满足 下 列 递 推 关系 式 


Jr -J5 iL, (2-74) 
Jr =S gate (2-75) 
JE =J gat (2-76) 
其 中 ， 一 步 预测 FIM 可 表示 为 
Jip Kin -Kin AeKa (2-77) 
Jr Kin" -Kin Au AB (2-78) 
Jip Se Kp — AAA (2-79) 


递 推 算式 〈2-74) 一 式 〈2-79) WIRA JaK, Janko 70, JO A. 

值得 一 提 的 是 ,根据 系统 条 件 的 不 同 ， 可 能 存在 不 同 的 PCRB 计算 方法 2" I， 另外， 
在 相关 噪声 条 件 下 ， 在 求解 PCRB 过 程 中 可 能 面临 矩阵 无 法 直接 计算 问题 ， 此 时 需要 进 
行 一 些 特殊 的 处 理 ， 关 于 这 一 点 可 参阅 文献 [24]。 


23 应 用 实例 


2.3.1 二 维 转换 量 测 目标 跟踪 
假设 二 维 平面 中 的 目标 运动 方程 满足 
x, = Fx, + Gw, , (2-80) 
式 中 : x, 7[x, X, Yk ár 为 k 时 刻 目标 的 状态 ; F 状态 转移 矩阵 ; C 为 噪声 输入 矩阵; Wi. è 
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为 零 均 值 高 斯 过 程 噪声 ， 且 


1700 TY2 0 
|o100| |T O0 adk 9 
boo iro rl mime «| 
0001 0 T 
设 观 测量 包含 目标 距离 和 方位 ， 观 测 方程 为 
[a peo KAQ He (2-81) 
Pk arctan(y,/x, ) Vok 


AP: v4. Vor 为 零 均 值 高 斯 测量 噪声 ，var[v ]=0}, var[v,,]=o;. 
采用 二 维 去 偏转 换 方法 进行 处 理 ， 得 


Xkc r, COS O; Hk x 
z c = = . E ( 2-82 ) 
E 网 $ MERE. | 网 


-cz/2 


式 中 


Hy x 77,0089, (e -e)s 4,,7nsing, (e 7* -e 
于 是 ， 去 偏转 换 测量 模型 可 表示 为 
Z = Hx, +v., (2-83) 
AF: H 为 观测 矩阵 ，v.。 为 去 偏转 换 量 测 噪声 ， 且 


1000 
a- 0.1 4 cov[v, , ]-R, , (2-84) 


R. 的 计算 方法 与 式 (2-46). 相同 。 

假设 T=2s ，9.=q,=2m/s*” ， 目 标 初始 位 置 和 速度 分 别 为 (60,80)km 和 ( -100, 
100V3)m/s ， 初 始 位 置 和 速度 估计 偏差 分 别 为 (30,40)m 、( 一 5,4)m/s ， 初 始 估 计 误 差 协 
方差 矩阵 

A lee 
式 中 
N-10000, 6,~N(x;0,5), X-diag([200? 50? 200 502]) 

进一步 假定 o,=20m ，o。=1 ， 采 用 去 偏转 换 卡 尔 曼 滤波 (CMKF) 对 目标 进行 跟 
踪 ， 所 得 结果 如 图 2.6 所 示 。 

由 图 2.6 中 的 结果 可 以 看 出 ， 经 去 偏转 换 处 理 后 ， 直 接 采 用 线性 条 件 下 的 卡尔 曼 滤 
波 算法 即 可 得 到 良好 的 跟踪 效果 ; 且 在 相同 的 距离 、 方 位 观测 误差 条 件 下 ， 跟 踪 误差 并 
没有 随 目标 距离 的 变化 而 发 生 明显 的 变化 。 由 图 中 的 结果 同样 可 见 ， 与 由 直接 转换 量 测 
得 到 的 结果 (图 中 o 型 线 ) 相 比 ， 经 滤波 处 理 后 目标 位 置 估计 误差 明显 减 小 ， 定 位 精度 
显著 提高 。 
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+ true 
-estimated 


axis-y(km) 





42 44 46 148 5 5 54 5 58 60 
axis-x(km) 


图 2.6 CMKF 目标 跟踪 示例 
2.3.2 ”跟踪 误差 理论 下 限 分 析 
仍 假设 二 维 平面 中 的 目标 运动 方程 满足 


Xk = Fx, , + Wii (2-85) 


AP: FRERE wa 为 零 均值 、 方 差 为 Q, 的 高 斯 过 程 噪声 ， 且 
jm 9 _ Ë |9 0 
j P j| EN k | 
i m ITA TR 
relo il o= an ia 
设 观 测量 包含 目标 距离 和 方位 ， 则 观测 方程 为 


| VX tk H] (2-86) 


arctan(y,/x,) | | Vo 
若 增加 径 向 距离 观测 ， 可 得 观测 方程 


15 V x Ty t V, k 
z,=| 9, |-hp(x, )*v,7| arctan(y,/x,) | 十 | vj (2-87) 
办 X, X, TY. Yk Vj k 








x, +) 

式 中 : Vas Vor 和 vk 分 别 为 零 均值 高 斯 测量 噪声 ，var[v,n]=o? , var[v,,]=o2, 
var[v, o; ° 

假设 T=0.25s ，g=2m/s* ， 目 标 初始 位 置 和 速度 分 别 为 (20,20)km 和 (60, — 200)m/s , 
初始 估计 【〈 先 验 ) 误差 协 方差 矩阵 总 =diag([300? 150? 300 1502]) ，o =50m ，cp=l ， 
or=0.5m/s ， 在 两 种 不 同 观测 模型 条 件 下 利用 式 (2-66) 一 式 (2-68) 分 别 计算 估计 误差 
的 理论 下 限 ， 此 时 有 
34 


x |= 
e [«] 
x [€ "ux 


r 
Ox r 


xr—axlr x 


r? ” 


0 
° r= J> +y? (2-88) 
0 
x 
一 0 
p 
y "EF 
- 0|, a-xxtyy (2-89) 
yr — aylr y 
r? r 


图 2.7~ 图 2.10 给 出 了 500 次 蒙特 卡 罗 实验 得 到 的 两 种 不 同 观测 条 件 下 的 状态 估计 
误差 理 论 下 限 ， 即 | 75 | 6912,30 。 由 图 中 的 结果 可 以 看 出 ， 与 传统 的 距离 一 方位 中 
踪 相 比 ， 引 入 距离 变化 率 观测 信息 可 改善 估计 精度 ， 同 时 提高 误差 收敛 速度 。 这 相当 于 
在 观测 目标 时 增加 了 新 的 约束 条 件 ， 从 而 增加 了 目标 状态 估计 的 确定 性 。 

不 过 ， 若 提高 距离 和 方位 的 观测 精度 如 or =20m ，o。=0.5")， 由 图 2.11 和 图 2.12 


中 的 结果 可 知 ， 由 距离 变化 率 信息 带 来 的 位 置 估计 性 能 改善 会 被 弱化 ， 但 其 对 目标 速度 
估计 精度 的 提高 仍然 是 明显 的 〈 尤 其 在 初始 跟踪 阶段 ?， 这 与 式 (2-89) 中 包含 了 目标 速 


度 信息 的 事实 是 一 致 的 。 


一 200 
E 


x 
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PCRB of position estimation for axis-x( 
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图 2.7 x 轴 方向 位 置 估计 误差 


PCRB of velocity estimation for axis-x(m/s) 





Time index 


图 2.9 x 轴 方向 速度 估计 误差 
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PCRB of position estimation for axis-y(m) 
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Time index 


图 2.8 》 轴 方向 位 置 估计 误差 


PCRB of velocity estimation for axis-y(m/s) 





Time index 
图 2.10 yy 轴 方 向 速度 估计 误差 
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一 一 一 hp(axis-x) 


PCRB of position estimation(m) 
PCRB of velocity estimation(m/s) 





0 20 40 60 80 100 0 20 40 60 80 100 
Time index Time index 


图 2.11 位 置 估计 误差 下 限 图 2.12 速度 估计 误差 下 限 


24 À 3 J # 


本 章 主要 总 结 了 非 线性 跟踪 中 常用 的 目标 运动 模型 及 传感器 观测 模型 ， 并 阐述 了 非 
线性 滤波 的 一 些 通用 性 能 评价 指标 。 

作为 一 类 重要 的 传感器 ， 雷 达 通 常 能 获取 目标 方位 和 距离 观测 信息 ， 但 这 些 观 测 是 
非 线性 的 ， 且 在 相同 的 观测 精度 下， 目标 离 雷达 越 远 ， 雷 达 对 目标 的 实际 定位 精度 就 越 
差 。 解 决 这 一 问题 的 一 条 有 效 途径 是 采用 去 偏转 换 方法 ， 将 非 线性 量 测 转 换 为 线性 量 测 
模型 ， 进 而 采用 卡尔 曼 滤波 算法 进行 跟踪 ， 本 章 对 去 偏转 换 方 法 进行 了 分 析 ， 并 给 出 了 
应 用 实例 。 

克拉 美 一 罗 限 表征 了 状态 估计 问题 所 能 达到 的 误差 理论 下 限 ， 对 评估 各 种 跟踪 算法 
的 性 能 具有 参考 性 、 示 范 性 的 作用 。 针 对 随机 系统 中 的 后 验 克 拉美 一 罗 限 (PCRB) 分 析 
问题 ， 本 章 阐述 了 非 线 性 条 件 下 的 误差 下 限 计 算 方法 ， 并 指出 线性 、 高 斯 系统 特殊 条 件 
下 信息 滤波 算法 与 PCRB 的 关系 ， 在 此 基础 上 还 分 析 了 混合 系统 状态 系统 状态 中 含 未 
知 参量 ) 的 PCRB 计算 问题 ， 最 后 结合 目标 跟踪 实例 ， 比 较 了 不 同 量 测 集 下 的 跟踪 误差 
下 限 。 
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UM MISS S RR 


贝 叶 斯 推演 为 动态 系统 的 状态 估计 问题 提供 了 统一 的 解决 方案 ， 但 是 由 于 其 最 优 估 
计 的 求解 过 程 需要 传播 整个 概率 密度 ， 这 只 有 在 线性 、 高 斯 条 件 下 才能 解析 递 推 。 对 于 
一 般 的 非 线 性 / 非 高 斯 系统 ， 通 常 难 以 得 到 解析 递 推 表达 式 ， 求 解 过 程 必须 采用 某 种 近似 
手段 。 若 将 系统 状态 的 概率 密度 等 效 近似 为 服从 高 斯 或 混合 高 斯 分 布 ， 则 由 此 得 到 的 滤 
波 器 常 被 称 为 高 斯 滤波 器 或 混合 高 斯 滤波 器 (或 高 斯 和 滤波 器 )。 

目前 存在 多 种 高 斯 滤波 器 ， 其 主要 区 别 在 于 近似 手段 的 不 同 ， 但 都 是 在 卡尔 曼 滤 波 
的 框架 下 得 到 的 〈 即 隐 含 了 对 高 斯 噪声 的 假设 )。 常 见 的 近似 手段 可 归纳 为 三 大 类 :; OR 
数 近 似 法 ， 其 主要 思想 是 用 非 线性 系统 函数 的 低 阶 展开 来 近似 非 线性 函数 本 身 ， 典 型 的 
有 扩展 卡尔 曼 滤波 器 (EKF)、 中 心 差分 卡尔 曼 滤波 器 (CDKF); 名 确定 性 采样 近似 法 ， 
用 确定 性 样 点 来 近似 系统 状态 及 其 非 线 性 系统 传递 后 的 分 布 ， 典 型 的 有 不 敏 卡 尔 曼 滤 波 
器 (UKF); @ 求 积 近 似 法 ， 用 各 种 数值 手段 来 近似 贝 叶 斯 滤波 递 推 式 中 的 多 维 积分 ， 典 
型 的 有 高 斯 一 厄 米 特 卡 尔 曼 滤波 器 (GHKF )、 求 容积 卡尔 曼 滤 波 器 (CKF) 等 。 

上 述 提 到 的 几 种 近似 手段 ， 都 无 一 例外 地 将 系统 的 状态 分 布 近 似 单个 多 维 高 斯 随机 
变量 (GRV )， 这 种 近似 对 于 常见 的 单 模 态 (Modality) 问题 是 合理 的 ， 但 对 于 多 模 态 
(Multimodal) E (如 含有 多 个 不 同 的 系统 状态 模型 /模式 ， 具 有 非 高 斯 过 程 / 量 测 噪声 
等 ) 这 种 近似 方法 显得 无 能 为 力 ， 这 就 是 为 什么 上 述 几 种 近似 方法 通常 只 用 于 求解 非 线 
性 、 高 斯 系统 中 的 滤波 问题 的 原因 。 解 决 多 模 态 问题 的 一 种 有 效 途 径 是 采用 多 个 不 同 的 
高 斯 分 量 进行 混合 近似 ， 这 种 方法 的 理论 基础 是 任何 一 种 复杂 的 分 布 都 可 能 用 混合 高 斯 
分 量 的 形式 达到 任意 精度 的 逼近 。 在 这 种 理论 指导 下 ， 各 种 混合 高 斯 滤波 (GMF) 的 方 
法 也 就 应 运 而 生 ， 典 型 的 有 GM-EKF. GM-UKF 等 。 

本 部 分 主要 闻 述 基于 解析 高 斯 /混合 高 斯 近似 的 非 线 性 滤波 方法 及 其 在 目标 跟踪 中 
的 应 用 问题 。 第 3 章 根据 函数 近似 原理 ， 重 点 介绍 了 两 种 重要 的 近似 方法 ， 即 泰勒 展开 
近似 和 函数 插值 近似 ， 并 分 析 了 基于 这 两 种 近似 方法 的 滤波 算法 。 第 4 章 从 函数 状态 分 
布 的 近似 入 手 ， 介 绍 了 不 敏 变 换 的 原理 ， 导 出 了 卡尔 曼 框 架 下 的 滤波 算法 及 其 算法 的 衍 
生 形 式 。 第 5 章 则 从 多 维 非 线性 函数 的 积分 式 求 解 这 一 角度 ， 分 析 了 求 积 型 卡尔 曼 滤波 
的 算法 和 实现 方法 。 第 6 章 针 对 多 模 态 问题 ， 系 统 介绍 了 基于 混合 高 斯 近似 的 滤波 原理 
及 通过 EKF, UKF 等 来 的 实现 的 方法 。 每 一 章 的 最 后 均 给 出 了 具体 的 应 用 实例 。 


第 3 章 ”函数 近似 高 斯 滤波 


通过 第 1 章 的 介绍 可 知 ， 卡 尔 曼 滤波 (KF) 为 线性 、 高 斯 条 件 下 的 递 推 贝 叶 斯 估计 
问题 提供 了 最 优 解 的 形式 。 显 然 ,在 一 般 的 非 线性 系统 中 ， 满 足 最 优 解 的 条 件 不 再 成 立 。 
大 量 的 研究 将 致力 于 寻求 各 种 不 同 的 近似 途径 得 到 次 优 解 ， 其 中 一 种 最 直接 、 自 然 的 途 
径 就 是 对 非 线性 系统 函数 进行 近似 化 处 理 。 

对 非 线 性 函数 的 近似 化 处 理 通常 有 两 种 重要 的 方法 : 一 种 是 利用 泰勒 级 数 展 开 
(Taylor Series Expansion, TSE) 技术 得 到 非 线 性 函数 的 多 项 式 近 似 表 示 ， 多 项 式 的 系数 
由 不 同 阶 次 的 偏 导 数 所 决定 ; 这 种 方法 的 特点 是 展开 点 只 有 一 个 ， 其 适用 的 前 提 是 非 线 
性 函数 具有 显 式 的 表达 式 且 偏 导 数 存在 。 另 一 种 方法 则 是 利用 插值 计算 实现 对 非 线性 函 
数 的 近似 ， 与 TSE 方法 不 同 的 是 ， 这 种 方法 用 差分 代替 了 微分 ， 不 需要 偏 导 数 运算 ， 同 
时 插值 点 不 局 限于 单个 点 。 

本 章 主要 分 析 和 探讨 基于 函数 近似 的 高 斯 滤波 方法 , 首先 利用 TSE 法 对 随机 变量 经 
过 一 般 非 线性 系统 的 统计 特性 进行 了 分 析 ， 在 此 基础 上 针对 一 阶 TSE 这 一 重要 特例 ， 导 
出 了 扩展 卡尔 曼 滤 波 (EKF) 算法 ， 分 析 了 其 近似 误差 ， 并 针对 特定 的 应 用 条 件 介 绍 了 
常用 的 EKF 衍生 算法 。 另 外 ， 作 为 一 种 重要 的 插值 近似 方法 ， 本 章 还 重点 阐述 了 二 阶 
Sterling 插值 原理 及 基于 这 一 插值 原理 的 中 心 差 分 卡尔 曼 滤 波 (CDKF) 算法 ， 并 给 出 了 
其 求 根 实现 形式 。 本 章 最 后 给 出 了 两 个 目标 跟踪 的 应 用 实例 。 


3.1 扩展 卡尔 曼 滤 波 


3.1.1 随机 变量 的 非 线 性 传递 
随机 变量 xe RWE x~ NX, P.) BERERA y = g(x) 传 递 后 有 


y=g(x)=g(x+e), e—N(0,P.) (3-1) 
对 上 式 在 x 处 进行 多 维 泰勒 展开 ， 得 
1 1 1 
y=8(¥)+D,g +7; Dg t Dog t Dig (3-2) 


AF: D,g 为 全 微分 算 子 ， 表 示 在 标 称 值 * 处 展开 ， 并 存在 误差 e 。 泰 勒 级 数 的 第 i 项 
可 表示 为 


| 
Lois el ye 之 g(x)| x-z (3-3) 
AP: e, 为 e 的 第 J 个 分 量 。 因 此 ， 第 i 项 级 数 是 关于 e 的 i 阶 多 项 式 ， 对 其 取 期 望 ， 得 


40 


epift] 


: Ə'g(x) 
hd |" w s s 1 (3-4) 
RP: pua Li) X (3, a,] 完全 展开 式 的 第 项 ， 如 
pi (0x,3,2,2) = Wi (ôx, + ôr, 展开 式 的 第 3 项 -am6n (3-5) 
m, 6, 为 e 的 n 阶 中 心 矩 ， 即 
m, en = E[ee ez flee, ---e,) pGodx (3-6) 


TÆ, Y 的 期 望 可 表示 为 


J= Elg +e- g0) + Ap?el+ E| a Dig ec Dg+.. | 


(3-7) 
erm FEN PE 
= gE) + |V co), . |+ | Die SP 8*7 | 
y-y -g(x*e)- Elg(x + e)] 
s La, Ta las w (3-8) 
vig Dpto Erg) z| Ds T e += i 


对 式 (3-8) 取 外 积 ， 并 假定 误差 是 对 称 分 布 的 《 即 奇数 阶 矩 为 堆 )， 则 有 
P, = El(y - 3X» - 3] G,P,G; -Z E[D2g]E[D2gT' 


i 2j T 
*E z. uta "LE Dig(D7g ) |- k e TO j=l T E| D? g |E[ 2; g] (3-9) 
Vi, j: ij>1 Vi, J: ij>l 
式 中 
G, = V,g(2J,., 
若 对 泰勒 展开 取 一 阶 线性 化 近似 〈 即 忽略 高 阶 项 的 影响 )， 则 


y == y" = g(x)* D,g (3-10) 
y ^ y" = g(x) (3-11) 
P, = P” -G,P,G; (3-12) 


3.1.2 扩展 卡尔 曼 滤 波 算法 


下 面 将 在 3.1.1 节 分 析 结 果 的 基础 上 给 出 EKF 算法 的 流程 ， 并 通过 一 个 简单 的 算 例 
分 析 EKF 由 线性 化 所 引入 的 误差 。 在 此 基础 上 ， 讨 论 EKF 的 几 种 衍生 算法 ， 并 指出 其 
可 能 存在 的 优点 和 不 足 。 

仍然 考虑 下 列 方程 所 描述 的 非 线性 系统 

x, 2 f(X) + W, (3-13) 
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y, = h(x,) + v, (3-14) 

AP: ÁO 和 已 D) 为 一 般 非 线性 函数 ， wx 和 vbw 分别 为 零 均值 过 程 噪声 和 测量 噪声 。 

由 1.2 节 中 的 分 析 可 知 ， 当 满足 以 下 两 个 条 件 时 : 

(1) w, 和 内 为 相互 独立 的 高 斯 噪声 ， 其 协 方差 分 别 为 CO. # R, ; 

(2) f,029 34., 的 线性 函数 ， h(x,) 9j x, 的 线性 函数 。 
递 推 贝 叶 斯 滤波 公式 存在 解析 解 ， 相 应 地 得 到 的 滤波 算法 为 线性 、 高 斯 条 件 下 的 最 优 卡 
尔 曼 滤波 算法 。 对 于 一 般 的 非 线性 方程 ， 式 (3-13) MA (3-14). 不 能 简单 写成 线性 表 
示 形 式 ， 后 验 分 布 的 解析 表达 式 也 就 难以 求 得 。 此 时 ， 一 种 可 行 的 方案 是 对 非 线性 函数 
fO. h() 进行 局 部 线性 化 ， 通 常 写 成 一 阶 泰勒 展开 的 形式 ， 即 


Of (x, 1) 
Ox 


k-1 


f (xi) Ke)t (x, 4 7 X44)*H.O.T 


Xk-1 =$] 





& f(x, )*F,X, , 
Ç oh 
h(x, )-h(*,, , TER 


k 


(x, — 44.) HH.O.T 





Xk-1 =k- 


党 h(,, )-H EX 
并 将 PG adl ua) > PG Haa) I ps, | yu) 近似 为 高 斯 分 布 ， 即 


P(Xi Ha) SN Go a, P) (3-15) 
p(x, lua) 8 N Ga quas Paci) (3-16) 
p(x, | yy) = N Go x, , P.) (3-17) 
其 中 
Xa = f(X,1) (3-18) 
Pik- =F,P, F; *Q, (3-19) 
X, = X4 + K,[y, -hlg )] (3-20) 
P, = Pox - K, H,P,, (3-21) 
K,=P,, ,H; (H,P,, ,H, +R) (3-22) 
p.20 ， H, 9 (3-23) 
x=Šy-1 x=Šupk-1 


X (3-18) 一 式 (3-23) 构成 了 扩展 卡尔 曼 滤波 (EKF) KAHE, Hp F, A H, 
为 多 维 状 态 泰勒 展开 得 到 的 Jacobian 和 矩阵。 完整 的 EKF 算法 凹 流程 可 概括 如 下 。 
Alg (3.1): Extended Kalman Filtering 
步骤 1: 初始 化 
WERF, P, Qo Ro 
步骤 2: 预测 
CD 计算 状态 模型 式 (3-13) 的 Jacobian 矩阵， 即 
F, -V, f(x , (3-24) 
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(2) 时 间 更 新 ， 即 


Xua = fi) (3-25) 
Pa = FP SF, + Q, (3-26) 
步骤 3: 修正 
(1) 计算 观测 模型 式 (3-14) 的 Jacobian 矩阵 ， 即 
H, - V hx)... (3-27) 
(2) 测量 更 新 ， 即 
K, = Pu He (H,Pj, + R,) ' (3-28) 
X, = X44 + K,[y, — h(,4)] (3-29) 
P, =P — K,H,P,, (3-30) 


3.2 EKF 的 误差 分 析 与 改进 方法 


3.2.1 EKF 的 近似 误差 分 析 
考虑 下 列 简单 的 随机 变量 非 线性 传递 过 程 
y-sg(x)sac +b, x—N(xw;x,o2) (3-31) 
AP: a, b 为 常数 。 


isk Sr gC) (x) =g%) (x) _ ， 则 根据 式 G- ~R (3-9), fü 


X=X 


y=g(x)+E le - xg? (x) G xL £^ 





(3-32) 

-a «E|2a(x - X)x * ax - xy |+b=a «ao? +b=a( +o2)+b 

o? = Qaxy e; - 1 E22a(x - xy] E[4a! x(x - xy ]+ Ela’ (x - x'] 
4 (3-33) 
- 4a! x? a? — a! (o2 y -- Aa! xE[(x — x) ] + a? E((x - x)*] 

由 于 x 为 高 斯 随机 变量 ， 故 E[(x-x) ]=0，E[(x -x)]=3(o?) ， 于 是 有 

c, - 2a'o* + 4a! x? o (3-34) 
显然 ， 取 一 阶 线性 化 所 得 的 均值 和 方差 分 别 为 

y" -g(x)-ax! «b (3-35) 
(0y)” - g^ GD)arg (x) - 4a^x'a; (3-36) 


为 了 更 好 地 说 明 线 性 化 方法 对 近似 精度 的 影响 ， 假 定 a=2，4b=1，x=2, 表 3.1 
对 不 同方 差 cx 条 件 下 的 近似 性 能 进行 了 比较 。 在 表 中 ,蒙特 卡 罗 近 似 法 采用 的 样本 数 为 
200000。 
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表 3.1 线性 化 与 蒙特 卡 罗 方 法 近似 精度 比较 





由 表 3.1 中 的 结果 可 以 看 出 ， 当 初始 误差 较 小 时 , 线性 化 方法 所 得 的 均值 和 方差 都 
比较 接近 真实 值 。 这 说 明 ， 初 始 误差 较 小 的 情况 下 ， 局 部 线性 化 是 合理 的 。 随 着 初始 
误差 的 增加 ， 线 性 化 结果 特别 是 方差 ) 与 真 值 间 的 差距 越 来 越 大 ， 这 就 是 EKF 算法 
对 初 值 敏感 、 滤 波 器 性 能 不 稳定 的 本 质 原因 。EKF 的 近似 误差 也 可 用 图 3.1 来 直观 地 
说 明 ， 由 图 中 可 以 看 出 ， 由 于 线性 化 过 程 存 在 较 大 的 误差 ， 致 使 后 验 均值 和 协 方差 估 
计 不 准确 。 

解析 法 (采样 ) 线性 化 (EKF) 


D 


y" = g(x) 
P," - GP.GT 
G = Vg). . 





y-E[gx)] 





图 3.1 线性 化 近似 误差 表示 


3.2.2 EKF 的 几 种 改进 方法 


1. 二 阶 EKF (泰勒 展开 在 二 阶 以 上 截断 ) 

由 前 面 的 分 析 可 知 ， 只 有 在 满足 局 部 线性 化 〈 即 标 称 状态 或 展开 点 与 真实 状态 充分 
接近 ) 的 条 件 下 ，EKF 才能 达到 较 好 的 效果 。 为 了 提高 估计 性 能 ， 对 fc, ) 和 h(x ) 分 
别 在 X, M X,, 1 处 进行 泰勒 展开 ， 并 取 展开 式 的 前 三 项 ， 即 可 推导 出 非 线 性 系统 的 二 阶 
滤波 公式 。 当 然 ， 在 展开 式 中 可 以 保留 前 四 项 或 五 项 。 但 实践 结果 表明 ， 二 阶 EKF 的 性 
能 要 好 于 EKF; 而 三 阶 、 四 阶 滤波 与 二 阶 滤 波 相 比 ， 性 能 改善 并 不 明显 ， 且 在 状态 量 维 
数 较 高 时 运算 相当 复杂 ， 故 一 般 很 少 采 用 。 

很 明显 ， 二 阶 滤波 器 需要 求 Hessian 和 矩阵， 这 将 使 计算 量 大 大 增加 ,不 利于 实时 应 用 。 
一 般 情况 下 ， 只 有 当 系 统 非 线性 程度 较 高 或 系统 状态 为 标量 的 情况 下 才 考 虑 用 二 阶 EKF. 
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2. IEKF (迭代 EKF) 
IEKF 的 基本 思想 是 ， 在 获得 大 时 刻 的 估计 值 总 和 估计 协 方差 呈 以 后 ， 将 其 代替 预 
测 值 X, i 和 预测 误差 协 方差 By 再 进行 EKF ah, Bp 


Ško = Šias Pro = Pp (3-37) 
x, ja Xa + K, [y 一 h(x D-H (E, x. Jl (3-38) 
P, 2 - K, H, |) Pas (3-39) 
式 中 
Oh(x) 
H, ; E ex 2 o5 K, | =P, Hi OR, Pa Bi, +R,) : (3-40) 
x=Xk, j 


EAR (3-38) ~R (3-40) 直到 满足 条 件 | 名 jw - +, S v, (V, ATERT, 
这 样 若 总 共 进 行 j=1,2,…,N RER, arti Sh X, = X, P, = P, o 

3. MGEKF (修正 增益 EKF ) 

EWED y, = h(x,) 满足 

h(x,) - h(X,4 ,) = G, Qu Xa Xx, 7 Xa) (3-41) 

也 就 是 说 这 个 非 线 性 函数 y, = h(x,) 可 以 写成 状态 x 的 线性 形式 ， 则 称 该 非 线 性 函数 
是 可 修正 的 入， 同时 也 就 可 以 应 用 MGEKF 滤波 算法 。 此 时 ， 滤 波 估计 误差 的 协 方差 
和 矩阵 为 





P, =U - K,G,(y,,,%,,-)]P,, aU — K,G, O mo Xu )] + K,R, K; (3-42) 
式 中 采用 Ym 代替 yi 是 因为 实际 中 只 能 得 到 带 有 噪声 的 测量 值 , 其 他 计算 公式 与 EKF 算 
m 
对 于 不 同 的 系统 ，MGEKEF 算法 具有 不 同 的 变换 矩阵 Gi Omot) EFAARAR 
f FI Gi (Yme $e) E: MGEKF 算法 的 关键 。 在 二 维 平面 y) 只 测 角 条 件 下 ， 有 


@, = h(x, ) = arctan(x, / y, ) (3-43) 
此 时 有 
G, -|o 0 COS Pry Faga | (3-44) 
1 "y 


在 三 维 空间 Gr, y, oz, ) 只 测 角 条 件 下 ， 有 


9, arctan(x, / y, ) 
[2 |= neo- arctan( [xd +y? /z,) (3-45) 
相应 地 ， 可 得 到 
COS Qu -sing,, 0 000 


F, Ê 
G= ~ * (3-46) 
cos @, sin[(@,, +@,)/2] cosg cos[(@,, +@,)/2] -sin 8, 000 


^ 


P, cos[(@,, — $,)/ 2] P, cos[(@,, — 9,)/2] Pk 
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式 中 

ñ = Xa Sin B, +a Cos, , P= ARA + (aa ) = ñ, sin @,, + £,44c096,, >» 
Ô, = arctan(£,, / 94). 

尽管 MGEKF 在 性 能 上 比 EKF 有 了 提高 , 但 对 于 一 般 的 观测 方程 ,修正 矩阵 不 存在 
或 很 难 求 得 ， 这 限制 了 该 算法 的 应 用 。 

4. MVEKF (修正 协 方差 EKF ) 


在 这 种 算法 中 回 ， 认 为 状态 涉 波 估计 3, 比 状态 预测 名 得 到 的 结果 更 为 精确 ， 从 而 
+ Oh 
利用 各 来 计算 Jacobian 矩阵 本 = 70D. 。 ;而 后 再 利用 该 矩阵 再 计算 一 次 滤波 误差 的 
协 方差 矩阵 ， 即 





K; = Pga (Hi) (H, P4 (Hy +R) 
P, =(I-K; H,)P,, 

上 述 过 程 可 认为 是 对 传统 滤波 误差 协 方差 矩阵 的 修正 。 仔 细 比 较 MVEKF 和 迭代 次 
数 为 2 的 IEKF 算法 不 难 发 现 ， 两 者 在 本 质 是 一 致 的 ， 即 通过 多 次 修正 提高 估计 精度 ; 
所 不 同 的 是 ，MVEKF 比 IEKF 少 了 一 次 更 新 状态 估计 的 环节 。 

表 3.2 对 EKF 几 种 衍生 算法 的 特点 作 了 简单 的 比较 ， 实 际 应 用 中 可 根据 需要 对 不 同 
的 算法 进行 合理 选择 。 

X32 JUft EKF 衍生 算法 特点 比较 









C wu |] — mr |] Mo | 
近似 精度 比 运算 开销 随 在 保证 观测 精度 必须 以 高 精度 观测 可 改善 协 方差 矩 t EX PM 
提高 显著 增 大 SIGERE 所 提高 稳定 性 存在 
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在 前 文 介 绍 的 EKF 及 其 衍生 算法 中 ， 尽管 针对 各 种 不 同 的 应 用 做 了 适当 的 修正 , 但 
其 理论 基础 是 相同 的 ， 即 用 TSE 去 近似 非 线性 系统 函数 。 这 类 方法 的 共同 特点 是 非 线性 
函数 的 近似 采用 的 是 单 点 近似 ， 且 通常 只 取 一 阶 逼 近 ， 因 此 近似 精度 受到 很 大 限制 ， 在 
系统 非 线性 程度 较 高 的 情况 下 更 是 如 此 。 

对 非 线性 函数 近似 的 另 一 种 有 效 方式 是 采用 插值 公式 , 与 采用 解析 求 导 的 方法 不 同 ， 
插值 公式 利用 了 若干 个 插值 点 的 函数 值 来 计算 。 根 据 不 同 的 需要 ， 可 能 有 不 同 的 插值 公 
x, 其 中 应 用 最 广泛 的 一 种 插值 方法 是 Sterling。 接 下 来 将 对 这 种 插值 方法 及 其 由 此 导出 
的 卡尔 曼 滤波 算法 进行 详细 介绍 。 


3.3.1 二 阶 Sterling 多 项 式 插值 


在 文献 [6] 中 利用 了 中 心 分 开 差 分 (Central Divided Difference, CDD) WAHAR, K 
为 Sterling 多 项 式 内 插 公式 。 对 于 二 阶 标量 情况 ， 该 内 插 公式 可 记 为 
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1 
g(x)= g(x)+ D,,g + 5; Dag (3-47) 


AF: Du. D 分 别 为 作用 于 函数 g(x) 的 一 阶 和 二 阶 中 心 分 开 差 分 (CDD) 算 子 。 
对 于 标量 情况 
f. AFG t h)- g(x —ñ) 
Dyg = (x ERES ON GG 
2 gx +h)+ g(x- h) -2g(x) 
n 


(3-48) 


D&g -(x-X) (3-49) 


AP: 天 为 中 心 差 分 步 长 ; 区 为 展开 点 的 先 验 均值 。 通 过 类 比 可 知 ，Sterling 内 插 公式 可 
认为 是 用 CDD 方法 代替 泰勒 级 数 展开 法 中 的 解析 求 导 。 

对 于 多 维 状态 量 ，8(x) 可 通过 对 先 验 变量 x e RI 的 “统计 解压 ”处 理 得 到 ， 其 处 理 
过 程 可 利用 下 面 的 线性 变换 来 实现 ， 即 


z=S8-x (3-50) 

(z) = g(S,z) = g(x) (3-51) 
AP: S, 是 变量 x 协 方差 矩阵 忆 的 Cholesky 因子 ， 即 

P. = S_ST (3-52) 


X (3-52) 中 假定 了 随机 变量 x 具有 均值 X=E[x]、 方 差 P. = E[(x —xXx - x) ]. 
ESI gO fl eO 的 泰勒 展开 是 等 效 的 ， 上 述 变 换 实 现 了 对 x 的 统计 解 耦 ， 这 是 因为 经 
变换 后 z 的 各 分 量变 为 互 不 相关 的 量 ( 具 有 单位 方差 )， 即 

P, = E(z —z)(z —z) ]= 1 (3-53) 

上 述 处 理 过 程 使 得 标量 中 心 差分 算 子 式 (3-48) 和 式 (3-49) 能 够 独立 地 应 用 于 &(z) 

的 各 个 分 量 ， 从 而 得 到 多 维 状态 量 的 中 心 差分 算 子 为 


ú L 
D, 8 =Q Amid)5(G) (3-54) 
i=l 
2 ~ q 2 42 Re 
D 8 - [P Aid; +> > A, A, (md))md, JEC) (3-55) 
i=] j=l q=1 
q*j 


AP: A, =(z 一 2); G=1,2,---,L) z-7 WEB i ES LONG m. d. d?2y 
别 为 均值 、 一 阶 差分 和 二 阶 差分 算 子 ， 其 定义 为 


m&G) - [ZE + he,)+ ZE —he,)) (3-56) 
d) - [ECE +he,)- &G -he)) G-57) 
dà) = [ŠG + he) * 8G - he) - 280) (3-58) 


AP: 6; 为 第 i 个 单位 矢量 。 
根据 式 (3-50) 和 式 〈3-51)， 易 得 
gG + he,)=g(S,[z + he, ])-g(S,z + hS,e,])-g(x + hs,]) (3-59) 
AP: osa 为 协 方差 矩阵 Cholesky 分 解 因子 的 第 i 9j, HD 
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s,7,e,-(S,),- (JF. ), 
3.3.0 ”插值 后 的 均值 和 方差 计算 


(3-60) 


AK RPE, IR R£ x 348. P, 7725 R0 LERSE x ZSTERCIEASTEPRI g(:) fe 


递 问 题 ， 即 
y = g(x) 
对 式 (3-61) 利用 多 维 变量 的 二 阶 Sterling 插值 公式 进行 展开 ， 得 


— SAS — rw. 
y=g(x)=8(2) = &2)* D, E +> Di E 


Amy: z28,x. 
于 是 了 的 均值 、 协 方差 、 互 协 方差 可 表示 为 
y = E[y] 
P, = EKy - Y» - ») ] 
P, = E(x-XXy-»)] 
根据 式 (3-62) 的 Sterling 插值 ， 可 得 均值 了 的 近似 值 为 


MEA f ku St Sosa 
ys E[£(2) * D, ë +— D$ g]= £(z) - E[— Dj 8] 
z 2 z 2 z 


L 
-8)* E; Qr A 48] 
i=l 
L 
-8G)*—5 DECE * he) * EE - he) - 280] 
i=l 
2 L 
= 3), EG +he)+8G - he] 
j=l 


AP, BEA, =z 一 z 为 零 均值 、 单 位 协 方差 随机 变量 。 
HR (3-59) RAR (3-66) 可 得 用 表示 的 均值 为 


_ k-L 1 二 w 
ye 8G)*7 LECE + hsa) +g- hss,)] 
i=l 


计算 协 方 差 矩 阵 的 近似 值 时 ， 可 采用 式 (3-64) 的 展开 式 ， 即 
P, = El(y- Y» -»)] 
= E[(y - £(X)X y - g(x) ]- Ely - £GO]ELy - £GOT 
= EG - EEY - £(2)) ]- Ely - ZEEL - š GZ) 
在 计算 上 式 时 ， 利 用 了 恒等式 
y = E[y] - E[y]+ 2(x) - g(x) 
= E[y] * (x) - E[g(x)] = (x) + Ely - g(x)] 
根据 式 (3-62)， 得 
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(3-61) 


(3-62) 


(3-63) 
(3-64) 
(3-65) 


(3-66) 


(3-67) 


(3-68) 


(3-69) 


w m 
y-&3)- D, &* Dg (3-70) 


A (3-70) BI y — g(z) 的 二 阶 Sterling 插值 近似 。 将 式 (3-70) 代入 到 式 (3-680, 14 
P, ~ El(D, ë +L D2 XD, ë + D2 8)"] 
i H bo r i (3-71) 

-E[D,, ë +> DA 8]EID,, ë +5 D4, £T 
类 似 前 面 的 处 理 ， 仍 假定 A, = z 一 z 为 零 均值 、 单 位 协 方差 对 称 随 机 变量 。 由 于 具有 
对 称 性 ， 所 有 奇数 阶 矩 为 零 ; 同时 ， 为 了 方便 计算 ， 将 舍弃 所 有 四 阶 项 的 分 量 


ELE goa | (该 项 包含 了 式 3-71) 展开 式 中 的 互 差分 ， 忽 略 四 阶 项 的 目的 是 为 
了 避免 因 z 的 维 数 增加 导致 的 计算 量 的 快速 增加 )。 基 于 上 述 假 定 和 处 理 ， 对 式 3-71) 
进行 展开 并 取 展 开 式 中 的 期 望 ， 可 得 协 方差 的 近似 值 为 
as 21e G hy) - 8G - hs 
[eG hs, ) - g (X - ha, YP 


2 
Lr : IgG hs, )* g(X - hs, ) - 


2g(X)l[g + hs, )+ g(x - hs, ) - 2g (x)]" (3-72) 
类 似 地 ， 可 得 到 互 协 方差 阵 的 近似 值 为 
Pu = E(x-xX»y-»)] 


~ E[(S,(z —z)X(D, ë + zB. £- EL D, gy] 





= E(GS,A XD, 8) 167 EIS, A, XDA, 8) 1-7 ESAE Do ë] 
= E((S,A,XD,, 8)'] STA 


L 
= 55.1 + he) - 8G - he] 


L 
= 2,5, 1G s, ) - gG hs, T! 
i=1 


式 中 利用 了 A: KRR ER ARRA R V BJ SECUTA IS SE BO TE JR , 

中 心 差分 步 长 参数 户 的 最 优 设置 取决 于 z = S; x 的 先 验 分 布 , DRAT, ATER 
勒 级 数 展开 得 到 的 均值 和 方差 与 近似 估计 得 到 的 结果 之 间 误 差 最 小 ， 所 与 z 的 峭 度 "应 
当 相等 。 对 于 高 斯 先 验 ， 最 优 的 h 取 值 为 V3 。 

一 旦 获得 随机 变量 经 非 线 性 变换 后 (经 Sterling 插值 ) 得 到 的 后 验 统计 量 ， 就 可 以 
很 容易 将 这 一 结果 应 用 于 卡尔 曼 滤 波 算法 框架 中 得 到 中 心 差分 卡尔 曼 滤 波 。 


© iE (Kurtosis): 其 定义 为 k=E[x — JJ/o* ， 为 随机 变量 x 的 均值 ，o 为 标准 差 ， 该 指标 用 于 描述 某 种 分 布 偏离 高 斯 
分 布 的 程度 ， 当 x 服从 高 斯 分 布 时 =3 。 
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333 中心 差分 卡尔 曼 滤 波 器 
中 心 差分 卡尔 曼 滤波 器 是 Sterling 插值 在 递 推 卡尔 曼 滤波 框架 下 的 直接 应 用 ， 用 
X (3-67)、 式 (3-72) MA (3-730 计算 经 插值 近似 后 的 后 验 均 值 、 协 方差 及 卡尔 曼 滤 
波 增益 ， 即 可 得 到 CDKF 滤波 算法 ， 具 体 流 程 可 概括 如 下 。 
Alg (3.2): Central Difference Kalman Filter ( CDKF ) 
步骤 1: 初始 化 
X, = E[x,] Po =E[l(xo 一 各)(xo 一 X) ] 
w = E[w] R, = E[(w — w)(w — w) ] 
y=Ely]  R,-E((v-9Yv-$)'] 
步骤 2: for 大 =1,2,… 
(OD 计算 协 方差 方 根 的 列 矢量 用 于 时 间 更 新 ， 即 
st =h(VP,,) i=1,2,,L, 
sz - (JR. ), i-12,---,L, 
(2) 时 间 更 新 式 ， 即 
-L,-L 


a h° w 
XQ = n d (X, ,, W, up) 


Mr y. a Xv. 1 十 全 P ha) t Fa si, Pu) 


+ = YU, FS, D) Fas Sri, Ui)] 
izl 
1 Y ^ xi = ^ xi — 2 
Pa 4n [f (X, + Ski Wu, ,) - (X44 78,5 Wu, )] 
izl 


l Tx E 2 
T uqa W S5) S(r W- Ski )] 


-1 


Uwe i iU. 1 SES, u, ,)+ f(&, Sk W, a)- 2 f (Xe, v, u, b 








M YUG, WSE MA) + fX, usi )-2/(%, uu 


《3) 计算 协 方差 方 根 的 列 矢 量 用 于 测量 更 新 ， 即 
si =h[ J Pya), A y S KE 
sk‘ =h(VR,) i=12,-,L, 
(4) 测量 更 新 ， 即 


50 


k? _ L, - L, 
Jak = me pY) 


+ XM Ces) + h(i si] 
EX bsp) hy 7s; )] 
i 


] £ A Xi — ^ x4 — 
Fi, Capite HSE 5)— f (Erka — sy S» 


ac SIG. py ts )- CETTE — Sy Di 





"a? 
+ Pus, tSp Sy) ha sp V) 2h, 1, 7) 
i=1 
h? =] c ^ 一 vi 人 一 v,i ^ 2 
t Y AG as + SK) + h(i ss — 87) 2AP) 
izl 
P E 1 a xi rh < Xi = h( < xi 一 人 1T 
Xk yk 52,5 [Aleka + Sk v) - h(X44 78. v)] 
K,-P, , P. 


XkJk ^ yk yk 
%, = Xa 1+ K, (y, 一 Ja- 1) 
P, = P4,,- K,P,, KT 


YkYk 


参数 说 明 : 上 述 计 算 公 式 中 ，h 宇 1 为 差分 步 长 (标量 )， 对 于 高 斯 变量 x， 该 参数 
的 最 优 取 值 为 h=V3 ; L... L, AL, 分 别 为 状态 变量 .过 程 噪声 和 观测 噪声 的 维 数 ; (VP ) 
表示 矩阵 方 根 Jp 的 第 i 列 。 另 外 ， 为 表示 方便 上 述 诸 式 中 用 [x 了 代表 xx, 


3.3.4” 求 根 中 心 差分 卡尔 曼 滤 波 器 


各 种 卡尔 曼 型 滤波 算法 中 ， 运 算 开 销 最 大 的 是 协 方差 矩阵 的 开 根 ， 出 于 提高 运算 效 
率 及 数值 稳定 性 等 目的 ， 文 献 [8] 采 用 了 求 根 形式 的 CDKF 算法 (SR-CDKF)。SR-CDKF 
算法 利用 Cholesky 分 解 形 式 直 接 传 递 和 更 新 协 方差 矩阵 的 方 根 , 与 标准 的 CDKF 算法 相 
HE, SR-CDKF 算法 具有 相近 或 略 优 的 估计 精度 、 稍 低 的 运算 开销 及 更 好 的 数值 稳定 性 。 

SR-CDKF 的 实现 主要 利用 了 QR 分 解 、Cholesky 因子 更 新 和 最 小 二 乘 解 这 三 大 线性 
代数 工具 ， 其 具体 的 实现 细节 如 下 : 

l. QR 分 解 

ARE A e RI" 的 QR 分 解 可 表示 为 

T 5 QR 

Xp: Q-RUU BERZE; RA E= fi B. N > L , REZAR R 29 Cholesky 
BT pP-AA'BEEE,. Bl R= At, WA R'R- AA! 。 这 里 利用 缩写 形式 gr 全 来 标识 天 
FER] RQ 分 解 并 返回 R 。 

RQ 分 解 的 运算 复杂 度 为 ONZ) , X} P = 447 直 接 利用 Cholesky 因 式 进行 分 解 的 复 
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杂 度 为 O(B/6)+ O(NL)。 
2. Cholesky 因子 更 新 
# S X p = 44T 原 始 的 下 三 角 Cholesky 因子 ， 则 Cholesky 因子 的 秩 -1 更 新 
P'= P + Jvuu! 
可 记 为 
$' = cholupdate(S, u,+v} 
3. 高 效 最 小 二 乘 解 
方程 (447)x= 4 的 解 对 应 于 超 定 最 小 二 乘 问题 Ax =b 的 解 , 这 可 以 通过 三 角 QR 
分 解 求 得 。 
通过 上 述 分 析 ，SR-CDKF 算法 可 概括 如 下 。 
Alg (3.3): Square-Root Central Difference Kalman Filter 
步骤 1: 初始 化 
X,-E[x,], S, -chol(cov[x,] , S, = JR, i £ =R, 
步骤 2: for k =1,2,.… 
COD 计算 协 方差 方 根 的 列 矢量 ， 即 
ss =a m, ss =| °; ç | 
Am = [X XE thse Xe ASe] 
(2) 时 间 更 新 式 ， 即 
Aya = fO I Su) 


2L 

^ m (m) qx 

Xa = > ul AC 
i-0 


S. -dr wf? E x AT ark ) y wg? (Aag: +A ark gs Xokk- )] 
式 中 
2 — — 
wf" = (h L. L,) ym di 


k? 
L=2(L.+L,); i=l, …, 2L 


(3) 计算 量 测 样 点 ， 即 
S 0 
x” = ika P So =) * 
ki = LX kk-l | 0 d 
Aa [Xa Xia + PS Xa ASi] 


(4) 测量 更 新 ， 即 
Jia = h(X,, 1 ua) 


2L 
NI (m) 
Juge => ui Xa 
i70 
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上 7qr [vum ica Juara ) yug? PTT tar gp 7 29064 )] 


P, Xk 一 Jur S. [X uai =: JLaar ea F 
K, =(P，/S，J/S， 


Xk Jk Yk 
%, = Xa + K,(y, — Pip) 
U-K,S,, 
S,, 7cholupdate(S, ,U, —1j 
式 中 
(m) (h =L SL) (m) 一 1 
É O P Lo +L) i= 
, =2XL +L); i21, 2L 
w= ya P -1 
| 475 4n 


3.4 应 用 实例 


3.41 径 向 速度 测量 跟踪 

为 了 对 EKF 算法 及 CDKF 算法 进行 测试 ， 考 虑 三 维 空间 中 的 跟踪 问题 ， 目 标 运 动 
模型 采用 直角 坐标 系 中 的 近 匀 速 模型 ， 设 大 时 刻 的 目标 状态 为 x, =[x Xy y z z] (k), W 
其 状态 演化 过 程 可 表示 为 





F 0 0 
x, = Fe, X44 m 0 F, 0 x, i +W, (3-74) 
0 0 RF 
Q 0 0 
Q, - cov[w, ]-q? | 0 Q@ 0 | (3-75) 
0 0 Q 


Ji F Ma ra 
aP. i oan :| 
T 为 采样 间隔 ， wx 为 零 均 值 高 斯 过 程 噪声 。 


采用 主动 雷达 传感器 ， 观 测量 包括 目标 距离 、 方 位 9 、 俯 仰 B 及 径 向 速度 六 (或 
称 距离 变化 率 )， 观 测 点 位 于 坐标 原点 。 观 测 方 程 可 表示 为 


ip 
X, Ty, tZk 


arctan(y, /x, ) T 
Vp,k 
pe=h(x )*v,7| arctan(z,/ | x2 9y2) |+ h4 (3-76) 
: x . Ok 
XX, TY, Vk *Z4Z, 


[+ 2 十 72 
Xk TYk TZk 
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AP: n 为 零 均 值 高 斯 噪声 ， 且 cov[v,]=R, . 
假定 目标 初始 位 置 为 (30,45,10)km ， 初 始 速 度 为 (一 160, -120,0)m/s ， 目 标 状态 的 初 
始 估计 误差 协 方差 矩阵 为 已 =diag([250?,100?,250",100?,250?,502]) ， 其 他 参数 见 表 3.3。 
采用 随机 初始 化 , 产生 500 次 实验 数据 , 利用 Alg (3.1) 和 Alg (3.2) 中 的 EKF 和 CDKF 
进行 跟踪 (两 种 算法 的 初始 条 件 完全 相同 )， 得 到 的 结果 如 图 3.2~ 图 3.5 所 示 。 
表 3.3 滤波 器 参数 设置 


UYTU; —Pr[Vy — Ta — 
m [| 2 | s [| o | o | 2 | | 


3.2 为 单 次 跟踪 的 航 迹 估计 示例 ， 图 33 为 500 次 仿真 得 到 的 归 一 化 估计 方差 
(NEES), K 3.4 和 图 3.5 分 别 为 位 置 和 速度 估计 均 方 根 误差 (RMSE)。 由 图 3.2 一 图 3.5 
中 的 结果 可 以 看 出 ， 在 本 实例 参数 设置 条 件 下 ，EKF 和 CDKF 性 能 几乎 完全 相当 ， 且 目 
标 位 置 和 速度 跟踪 都 有 明显 的 收敛 过 程 ， 这 说 明 在 该 条 件 下 两 种 算法 的 近似 精度 是 等 效 
的 。 较 之 位 置 估 计 ， 速 度 估计 的 收敛 速度 更 快 ， 这 是 因为 增加 了 径 向 速度 观测 量 信 息 。 
另外 ， 从 图 3.3 中 的 结果 看 ， 跟 踪 收敛 后 两 种 算法 的 NEES 均 在 6 附近 以 微小 的 幅度 起 
伏 〈 理 论 值 为 6)， 说 明 跟 踪 算法 具有 良好 的 估计 一 致 性 。 
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axis-y(m) Time(s) 
图 3.2 单 次 跟踪 示例 图 3.3” 归 一 化 估计 方差 
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图 3.4 位 置 均 方 根 误差 图 3.5 速度 均 方 根 误差 
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3.4.2 ”椭圆 扩展 目标 跟踪 


考虑 直角 坐标 系 中 二 维 平面 内 的 目标 运动 ， 若 目标 状态 (包括 位 置 和 速度 ) 为 
xx =[x Xy y] (k)， 则 当 目 标 作 近 勾 速 (CV) 运动 时 ， 其 状态 方程 可 表示 为 


x, = Fox, É a Ia +W, (3-77) 
0 F 
2 |Q d 
i a ° (3-78) 
Q, =cov[w,] = q k Q, 
式 中 
Ji T JR Tn 
r-o j o-an p | 
T 为 离散 采样 间隔 。 
采用 雷达 观测 得 到 目标 距离 二、 方位 由 及 径 向 长 度 关 信息 ， 观 测 方程 为 
yxk tyk n 
y,7h(x,)*v, = arctan(y/x, ) 十 | vçk (3-79) 
Lycos’ ó, *(blay'sin^d, | LY 





AF: 《为 目标 主轴 向 长 度 ，b/a 为 椭圆 目标 的 短 轴 与 长 轴 长 度 比 ; v, 为 零 均值 高 斯 噪声 
Hecov[v,]=R, ， 从 为 方向 角 【〈 雷 达 视 线 与 目标 速度 方向 的 夹 角 ) H. 

XX, + Pk Vk 

2 [:3,.2 3-80) 

Vx YN XR 

进一步 假定 b/a 已 知 ， 而 4 为 未 知 量 ， 为 此 需要 将 4 与 x 组 合成 扩展 状态 量 
xe=[x, L] ， 此 时 跟踪 算法 中 的 状态 模型 为 
F 0 0 
0 F 0 
0 0 1 


@, = arccos 


x, = Fx; tw; = Xi tW, (3-81) 








式 中 
we=[we Wl, covDw?]-Q; -diag([Q, q;7^]) 
对 于 式 (3-79) MR (3-81) ARERR, HFA) cos? ó, *(b/ay'sin^g, 是 
系统 状态 的 隐 函 数 ， 在 使 用 EKF 算法 进行 跟踪 时 ， 求 Jacobian 矩阵 是 较为 复杂 的 ， 此 时 
可 采用 偏 导 的 链 规则 来 计算 ， 即 


ICH) _ INH) 06 
ôr ðf ôr’ 


实验 中 ， 假 定 目标 初始 状态 为 xe=[20km 10V3m/s 15km 10m/s 50m, HERRA 
的 初始 估计 误差 协 方差 矩阵 为 已 =diag([1002,10?,1002,102,102]) ， 其 他 参数 设置 见 表 3.4。 
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T=x,X.y.y (3-82) 


表 3.4 仿真 参数 设置 
| | e | zr | = | «e | = | £ | w | 
| Om? | ooms | 2s | sm | os | 2m | sm | os | 
根据 以 上 条 件 ， 采 用 随机 初始 化 进行 500 次 独立 实验 ， 并 利用 EKF 和 CDKF 进行 
跟踪 ， 所 得 结果 如 图 3.6 一 图 3.10 所 示 。 
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图 3.6 单 次 跟踪 示例 
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由 图 3.6 一 图 3.10 可 以 看 出 ， 与 前 一 个 示例 中 的 结果 明显 不 同 ， 本 实例 中 EKF 和 
CDKF 的 性 能 差异 较 大 ， 这 一 点 无 论 在 单 次 跟踪 还 是 多 次 统计 结果 中 都 有 具体 体现 。 究 
其 原因 ， 主 要 是 观测 方程 中 的 目标 长 度 观 测 函 数 具 有 较 强 的 非 线性 ， 在 这 种 条 件 下 基于 
一 阶 泰勒 展开 的 EKF 算法 近似 误差 较 大 ; 同时 , 目标 运动 状态 与 长 度 信息 在 求 取 Jacobian 
和 矩阵 时 存在 强 耦 合 ， 进 一 步 引 入 了 误差 源 。 相 比 之 下 ， 具 有 二 阶 插值 精度 的 CDKF 算法 
近似 误差 要 小 得 多 且 不 存在 偏 导 数 计算 环节 ， 因 此 估计 性 能 要 比 EKF 好 得 多 。 从 图 3.10 
中 的 结果 看 ， 在 CDKF 滤波 算法 收敛 后 其 NEES 接近 理论 值 5， 说 明 在 本 实例 条 件 下 仍 
有 较 好 的 估计 一 致 性 。 

另外 ， 为 了 比较 算法 的 复杂 度 ， 表 3.5 给 出 了 两 种 算法 在 Matlab7.12 环境 、2.8GHz 
主 频 和 2.75GB 内 存 条 件 下 (后 面 章节 中 的 实验 均 采 用 这 一 环境 ) 500 次 仿真 的 运行 时 间 
统计 。 由 此 可 以 看 出 ， 两 种 算法 都 具有 较 好 的 实时 性 ， 但 相 比 之 下 ，CDKF 的 算法 复杂 
度 稍 大 〈 本 例子 中 约 为 EKF 的 4 倍 )。 


X35 两 种 算法 运行 时 间 比 较 


算法 
执行 时 间 


STD/ (srun) 


3.5 本 章 小 结 


EKF 的 提出 是 为 了 简化 待 处 理 的 非 线 性 系统 以 适应 线性 卡尔 曼 滤波 器 的 要 求 ， 即 
通过 在 当前 状态 线性 展开 来 近似 非 线 性 函数 。 理 论 和 实践 均 表 明 ， 在 系统 非 线 性 程度 
不 太 高 且 初 始 误差 不 大 的 情况 下 ，EKF 可 得 得 到 良好 的 估计 性 能 〈 即 在 满足 局 部 线性 
化 条 件 下 其 性 能 是 可 靠 的 )。 然 而 ， 当 应 对 强 非 线性 系统 时 ，EKF 常常 难以 调节 ， 有 时 
甚至 发 散 ; 同时 ， 由 于 涉及 偏 导 数 计算 ， 这 种 算法 只 适用 于 具有 显 式 表达 式 且 偏 导数 
存在 的 系统 方程 。 

作为 非 线性 函数 的 一 种 有 效 近 似 手 段 ，Sterling 插值 方法 具有 近似 精度 高 、 免 偏 导数 
计算 等 优点 ， 以 此 为 基础 的 CDKF 算法 在 处 理 非 线性 程度 较 高 时 的 滤波 问题 中 较 EKF 
算法 精度 具有 明显 的 优势 ， 但 其 算法 复杂 度 要 比 EKF 高 。 因 此 ， 实 际 应 用 中 需要 根据 具 
体 的 问题 选择 合适 的 方法 ， 另 外 ， 从 工程 实现 的 角度 看 ， 为 了 提高 系统 的 稳定 性 ， 采 用 
求 根 形 式 的 实现 方法 ”更 为 合适 。 

需要 指出 的 是 ， 对 于 EKF， 除 了 前 文 提 及 的 几 种 常见 衍生 算法 以 外 ， 还 存在 衰减 记 
忆 EKF""、 强 跟踪 EKF} "等 几 种 重要 的 衍生 形式 。 此外， 本 章 在 介绍 各 种 算法 时 ， 均 以 
高 斯 白 噪声 为 假设 前 提 ， 关 于 色 噪 声 条 件 下 的 卡尔 曼 滤波 算法 ， 可 参阅 文献 [12]， 在 此 
不 再 细 述 。 
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第 4 章 ”确定 性 采样 近似 高 斯 滤波 


从 第 3 章 的 分 析 中 可 以 看 出 ，EKF 及 其 衍生 算法 尽管 在 线性 化 过 程 中 选择 的 展开 点 
不 同 ， 但 都 无 一 例外 地 要 计算 Jacobian 矩阵。 用 泰勒 展开 来 近似 非 线性 函数 的 方法 往往 
在 对 状态 后 验 分 布 的 估计 中 引入 较 大 的 误差 ， 这 一 问题 在 模型 的 非 线 性 程度 较 高 时 更 为 
突出 , 即 此 时 泰勒 展开 的 高 阶 项 影响 变 得 更 为 显著 。 由 此 带 来 的 直接 后 果 是 近似 精度 低 、 
协 方差 易 出 现 病 态 ， 严 重 时 致使 滤波 器 发 散 。 为 避免 以 上 问题 ， 文 献 [13] 中 提出 一 种 新 
的 基于 确定 性 样 点 计算 的 滤波 算法 ， 由 于 该 算法 建立 在 不 敏 变 换 (Unscented Transform， 
UT) 基础 之 上 ， 称 为 不 敏 卡尔 曼 滤波 Unscented Kalman Filter, UKF) "4'3, 

与 EKF 不 同 的 是 ，UKF 并 没有 近似 非 线性 状态 和 观测 模型 ， 而 直接 利用 真实 的 系 
统 模 型 。 与 EKF 相同 的 是 ，UKF 也 将 状态 近似 为 高 斯 随机 变量 “GRV)， 但 此 时 这 些 
GRV 用 一 组 有 效 的 采样 点 集 来 表示 ， 这 些 样本 点 能 捕获 到 GRV 的 均值 和 协 方差 。 当 状 
态 量 通 过 实际 的 非 线 性 系统 之 后 ， 后 验 均值 和 协 方差 可 以 精确 到 三 阶 (泰勒 展开 )， 且 对 
任何 非 线性 系统 都 有 这 一 近似 精度 。 同 时 ， 由 于 不 需要 推导 和 计算 复杂 的 Jacobian/ 
Hessian 矩阵 , 基于 UT 的 UKF 算法 更 便于 实现 .UT 也 可 视 为 随机 线性 化 的 离散 实现 "9， 
与 EKF 中 的 局 部 线性 化 不 同 ， 随 机 线性 化 利用 了 系统 在 状态 空间 中 的 多 点 信息 ， 这 也 正 
好 说 明了 UKF 精度 高 于 EKF 的 原因 。 ' 

本 章 首先 介绍 不 敏 变换 的 基本 原理 ， 并 对 随机 变量 在 非 线性 系统 传递 过 程 中 的 误差 
进行 了 分 析 ， 在 此 基础 上 , 结合 卡尔 曼 滤 波 框架 得 到 了 UKF 算法 。 针 对 具体 的 应 用 条 件 
和 特定 的 系统 模型 , 本章 还 研究 了 UKF 的 几 种 重要 的 扩展 形式 , 如 求 根 UKF、 迭 代 UKF, 
简化 UKF 及 联合 估计 UKF 等 ， 最 后 给 出 了 几 个 典型 的 应 用 实例 。 


4.1 不 敏 变 换 与 UKF 滤波 


4.1.1 不 敏 变 换 UT 


UT 是 一 种 计算 随机 变量 经 非 线性 传递 后 的 统计 特性 的 方法 。 考 虑 一 个 工 维 随 机 变 
量 x 经 过 非 线性 变换 后 得 到 = g(x) ， 并 假设 x 的 均值 和 方差 分 别 为 xX 和 P.。 为 了 计算 
变量 了》 的 统计 特性 ， 需 要 选取 2L+1 个 样 点 X 。 这 些 样 点 的 取 值 规则 为 


Z, = x 
z, = x+(J(L + x)P,), (i21, …, L) (4-1) 
z2X-QKL*-x)B), (G-L*L--,2L) 
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ut"? = k/(L-^ x) 
w(? — k (L + k) (i=1, …, 2L) (4-2) 


p ) =w® =1/(2L 42x) 


RP: ( JP.) (VP.)=P,: * 用 于 确定 样 点 与 均值 之 间 的 距离 ，(VP.) 表示 矩阵 方 根 
的 第 ; 列 。 将 这 些 样 点 通过 非 线性 变换 ， 即 


[= g(x,) (=0, 1, …，27) (4-3) 
变量 了 的 均值 和 方差 可 计算 为 
2L 
y - 2 ui", (4-4) 
i-0 
2L 
P, =u -IHR -IF (4-5) 


4.1.2 UT 近似 误差 分 析 
为 分 析 UT 的 近似 精度 ， 首 先 选 择 一 组 样 点 
z, = X £(J(L* OP,), x (L4 x)e, = x £ o, (4-6) 
RP: e ABER P, B5B i PU, "ne =P,, o, - J(L* &)e,. 
将 式 (4-6) 代入 到 多 维 泰勒 展开 式 (3-2), # 


_ l l 
X = g(x) = g(¥)+ D,g + D;8 +— 


s Dog eq Dig (4-7) 
y = E[g(x + e)] 


m 1 
-ai 2d X(L 4 Kk) 


>. gi) D gtt 


(4-8) 
— 3| ^^ 3 "ap 





2L 
l d b oss l ç |) 
x)+ > D g+—D g+—D; g+::: 
-& ERS E ! v 4! T 6! "E 


i=l 





又 
2L 
S Dig- >[D, (D, g) ]= S [v a+ ei [LAVE] ss | 
" (4-9) 
- (LY [ee yao]. ] - 205 + (Y P.C), 
i=l 
于 是 
y UT =g(s)+L(VTP V)g(x) * : 3E D$ g+— —D;gt- J (4-10) 
2 2 xx Q(L-x)£ 4 %” 61 ^ 
同 理 
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PU = El(y-y)(y- y) ] 
i; l E j 
=G; P.G; (Gs, G; (6. P.G; ) 


T 
fx 之 = 之 ings 8) (4-11) 


Vi, j: ij>1 
x pk (Dass) 
> >. LAS 
Vi, j: ij»l 

对 比 式 (4-100. AA (3-7) 不 难看 出 ， 当 x 为 高 斯 变量 时 ， 经 UT 求 得 的 后 验 均 值 
可 以 达到 三 阶 精度 ， 仅 在 四 阶 以 上 展开 项 存在 误差 ， 同 理 ， 对 比 式 (4-11) MA (3-9) 
可 以 看 出 ， 后 验 协 方 差 也 可 精确 到 二 阶 。 相 比 之 下 ， 线 性 化 所 得 的 结果 无 论 在 均值 还 是 
协 方差 都 只 有 一 阶 精 度 。 由 推导 过 程 同样 可 以 看 出 ，UT 所 能 达到 的 精度 不 依赖 于 方程 
的 具体 形式 。 这 就 说 明 ， 对 于 任意 的 非 线 性 系统 ， 都 能 达到 同样 的 近似 效果 。 

为 提高 变换 精度 ， 减 小 高 阶 矩 近 似 误 差 ， 可 通过 引入 一 尺度 因子 w (O<a <l), 使 
新 的 变换 样 点 满足 名 = Xo tal-X) ， 并 计算 相应 的 权 值 来 实现 。 这 种 方法 称 为 变 尺 
BE UT (Scaled UT)， 其 具体 实现 过 程 可 参阅 文献 [17]、 文 献 [18]， 这 里 不 再 著述 。 

作为 比较 ， 图 4.1 示 出 了 几 种 不 同 变换 方法 的 近似 误差 示例 。 由 图 中 的 结果 可 以 看 
到 ，GRYV 经 非 线 性 系统 传递 后 ， 由 一 阶 线性 化 方法 得 到 的 后 验 均 值 和 协 方差 估计 与 真 值 
相差 甚 远 ， 相 比 之 下 ， 由 UT 得 到 的 估计 与 真 值 则 较为 接近 ， 这 说 明 UT 的 近似 精度 要 
明显 高 于 线性 化 方法 。 


解析 法 (采样 ) 线性 化 (EKF) UT(UKF) 
协 方差 | : 
| 
7s) 
y"-g(x) | 
"b GT 
=g) 加 权 采 样 均值 





y o 
a “真实 均值 R -六 
* 7 : 真实 协 方差 Gp GT 


图 4.1 非 线 性 变换 比较 


下 面 仍 已 以 式 〈3-31) 的 非 线性 方程 为 例 ， 分 析 UT 所 能 达到 的 精度 。 此 时 工 =1， 
HÑ (4-3) 可 得 到 3 个 样 点 ， 即 
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X={x,xX+0,,X—0.} (4-12) 


£X (4-3) 传递 后 得 
Y - (ax? * ba(x * 4x 1o, Y ^ b,a(x -Vr 1o, ). +b} (4-13) 
再 由 式 (4-4) 和 式 (4-55, f8 


yur = 


pU = 











ax + Isl 

m EE cula K+ 10,) +b]+ 

[a(x -Vx + lo, )? k. 
Ñ ax? +b) +——— 2 ma MA * b) * 2a(x *1)o?] 
-a(x?*o2)*b (4-14) 
= 一 (oa oy + XP "i {[2x Vx +10, + ko?] + 

[2x4 k +10, + ko? jT ) 
E alas 
roii Am E * (& -- )o? + x^a*] 
- kxa^o? +4a x^ c? | (4-15) 


由 此 可 见 ， 经 UT 得 到 的 均值 与 真实 值 完全 一 致 ， 而 方差 的 近似 精度 与 因子 * 的 选 
择 有 关 ， 在 本 例 中 当 k =2 时， 所 得 的 协 方差 也 是 精确 的 。 


4.1.3 UKF 算法 


考虑 如 下 一 般 形 式 的 非 线性 系统 方程 : 


x, = (xi) (4-16) 
y, 7 h(x,,v,) (4-17) 


Ap: SOMALO 为 非 线性 函数 ，mw, e RI Av, es 玉 ”分 别 为 过 程 噪声 和 观测 噪声 ， 且 相 
应 的 协 方差 矩阵 为 CO 和 R, 。 
将 4.1.1 节 介绍 的 UT 融合 到 KF 中 ， 便 可 得 到 如 下 UKF 算法 。 


Alg (4.1): Unscented Kalman Filtering 


步骤 1: 初始 化 


x = E[x,] (4-18) 

P, = E|(x, - &)(x — 2) | (4-19) 

&-E[x]-[s] e o] (4-20) 
P 0 0 

B; = E (xt -&t)(st - s) |- 0 Q, 0 (4-21) 
0 0 R, 








步骤 2: 样 点 计算 
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aic atfer) | 
步骤 3: 时 间 更 新 
Xii 7 flatu) 


2L T 

^ m x 

Xu = Y wl A i,k|k-1 
i-0 


nin eh [uc ta T 
= 
Pp = > 

ERA. 测量 更 新 


2L, 
Fr = > [Mur Jk IX ua Ex Pp Y 
i=0 


2L 
€ (c) ^ ^ T 
P XQ luf [Xii 7 Xy allia 7 Jil 


_ E 
K, S FE FE, pk 
%, = Xa * K,(y, 一 Jac) 


P, = Pix- - K,P K; 


YkYk 


式 中 


LS Q” Q, x 2px w' v], L= l, +L,+L, 


42 H UKF 算法 


(4-22) 


(4-23) 


(4-24) 


(4-25) 


(4-26) 


(4-27) 


(4-28) 


(4-29) 


(4-30) 
(4-31) 
(4-32) 


受 有 限 字 长 等 因素 的 影响 , 工程 应 用 中 为 了 避免 因 舍 入 误差 导致 的 数值 不 稳定 问题 ， 
常常 采用 求 根 形式 的 UKF (SR-UKF) " "， 来 避免 矩阵 求 逆 操作 ， 提 高 滤波 算法 的 数值 


稳定 性 。SR-UKEF 的 算法 流程 可 概括 如 下 。 
Alg (4.2: Square Root Unscented Kalman Filtering 
步骤 1: 初始 化 
€, = E[x,] 


P, =E| (x -名 )(xo -名 ) | 





S, = chol(P?) 
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w™ = AKN + À) 
ut? - A/(N - A) - (02 à? +m) (i=1,2,--,2N) 
u( 2 w(? =1/(2N 24) 
步骤 2: 样 点 计算 
tarli tr- Sa 35-14] 
步骤 3: 时 间 更 新 
Xia = F Xia + Xk- 


2N (m) 

^ mes m Xx 

Xa =9 u Ai kk-i 
i-o 


S, =qr [e LITE 一 总和 | 


S, = cholupdate Ís; a kk-1 7 Xia ) uf?) 
Za Tha) zi 
、 2N (m) 
Zkk-1 = 2 Zikk- 
步骤 4: 测量 更 新 
D, = gr [^e (Zara 7 is )] 
D, = cholupdate x (una 73a): uj 


2N 

F x 一 Zw" Da. — Še- [Z pn a Za T 

i-0 

%, = Xin * K,(z, - £u) 

U - K, : D, 

S, = cholupdate| S; ,U,-1) 
AP: = QUY Q) x =[x w v], N=d,+d,+d,, A-aXN« 
K-N, y=VN+A; QQ 、K、7 为 控制 权 值 分 布 的 参数 ， 具 体 取 值 将 在 仿真 中 说 明 ; 


W, wO 为 各 样 点 的 权重 ;起 A P ARE x 的 估计 及 协 方差 矩阵 qr 人 0 表示 和 矩阵 的 
QR 分 解 chol(], cholupdate(-) 分 别 表示 矩阵 的 Cholesky 分 解 和 更 新 。 


43 IEIR UKF 算法 


与 EKF 及 其 衍生 算法 相 比 ，UKEF 算法 稳定 性 有 了 不 同 程度 的 提高 。 但 是 ， 在 很 多 实 
际 应 用 中 〈 特 别 是 在 被 动 目标 跟踪 的 应 用 条 件 下 )， 受 初始 条 件 〈 误 差 较 大 ) 和 可 观测 条 
件 (可 观测 性 较 弱 ) 的 影响 ,标准 的 UKF 算法 存在 着 反应 速度 慢 、 收 敛 精度 不 高 的 问题 。 
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在 文献 [20] 中 指出 ， 由 IEKF 产生 的 迭代 序列 总 等 同 于 Gauss-Newton 迭代 产生 的 
序列 , 因此 迭代 过 程 具有 全 局 的 收敛 性 。 理论 上 , IEKF 的 性 能 必定 优 于 EKF 和 MVEKF, 
但 文献 [21] 的 研究 表明 ， 实 际 应 用 中 并 非 总 是 如 此 。 其 原因 之 一 是 文献 [20] 中 的 结论 是 在 
局 部 线性 化 条 件 完全 满足 的 情况 下 得 出 的 ， 即 展开 点 与 真 值 非常 接近 。 但 在 很 多 应 用 场 
合 特别 是 被 动 目标 跟踪 应 用 中 ， 由 于 初始 误差 一 般 都 比较 大 ， 这 一 假设 条 件 不 能 成 立 。 
原因 之 二 是 实际 中 测量 误差 不 能 无 限 小 。 从 状态 更 新 公式 看 ， 很 明显 状态 量 的 每 次 修正 
是 通过 观测 量 完成 的 ， 因 此 收敛 特性 必然 严重 依赖 于 测量 精度 。 文 献 [21] 证 实 ，IEKF 对 
测量 误差 非常 敏感 ， 故 若 在 迭代 过 程 中 对 状态 量 的 修正 完全 依赖 于 观测 值 ， 很 可 能 不 但 
达 不 到 改善 滤波 性 能 的 目的 ,相反 会 导致 严重 的 性 能 恶化 ,原因 之 三 是 尽管 Gauss-Newton 
方法 具有 全 局 的 收敛 性 ,但 不 能 确保 似 然 的 增加 中 。 此 外 ,门限 芒 ; 的 选择 对 迭代 算法 的 
成 败 至 关 重 要 ， 但 如 何 选 择 合适 的 也, 却 是 不 好 解决 的 问题 。 

受 IEKF 算法 的 启发 ,同时 考虑 到 UKF 本 身 所 具有 的 优势 , 一 个 自然 的 想法 是 如 果 将 
EREE UKF 相 结合 ， 可 能 会 带 来 滤波 器 性 能 的 改善 。 但 是 由 于 UKF 是 基于 样 点 变换 
的 方法 ， 如 何 使 迭代 收敛 是 个 问题 ， 同 时 ， 鉴 于 传统 EKF 存在 的 潜在 问题 ， 必 须 采 取 合 
适 的 迭代 策略 使 其 扬长 避 短 ， 真 正 发 挥 迭 代 算 法 的 优势 。 在 接 下 来 提出 的 IUKF 算法 中 将 
解决 这 些 问 题 ， 算 法 中 采用 了 不 同 于 IEKF 的 迭代 策略 ， 其 主要 流程 可 概括 如 下 4。 

Alg (4.3): Iterated Unscented Kalman Filtering 

步骤 1: 在 上 (k2>1) 时 刻 ， 通 过 式 (4-22) ~ (4-32) 所 示 的 UKF 算法 得 到 状 
AE x, 和 相应 的 协 方差 矩阵 县 o 

步骤 2: 4 Xue = Xa , Po = #kjk-1 H£, =$; , P, =P. FjiF2 g=1, j-2. 

步骤 3: 根据 新 的 协 方差 矩阵 产生 样 点 。 


zu =a Ea tT HORS (4-33) 


P, 0 
at —ra4T T * =: k,j-1 
X. A [X 0] ,Pa -| 


0 R, 
步骤 4: 对 式 (4-24) ~R (4-32). 重新 计算 。 


式 中 


2 

Xi " Y ux, (4-34) 
i-0 

X, = hx.) (4-35) 
2L, 

3a = Duy, j (4-36) 
2L, 

P, =P WON- Pr TY -PaT (4-37) 
i-0 
2 

Boki =% ui [z EEP e e E (4-38) 

K,, = Py, Ps; (4-39) 


X, j = Xu; +g: K, (y, - Puy) (4-40) 
P, = Bj -K ,P,, s Kk, (4-41) 
AP: 下 标 j 表示 第 j 次 迭代 ; ,表示 变换 点 集 的 第 i 个 分 量 。 
WES. 定义 如 下 3 个 式 子 ， 即 
D Pry = h(X, ;) 
Q x, ; =f = i 
(9 Prj = X = 
步骤 6: 若 下 式 成 立 〈 见 附录 C) 
X, PL) a3; + R. Pey Jia Ri Jrj- (4-42) 
HJj&€N, WJ&zg=n: g, j=j+ RRHH 3; 否则 执行 步骤 7。 
步骤 7: 若 式 (4-42) 不 成 立 或 了 过 大 (7 N ) 则 和 迭代 终止 , 3E X, = X,,, P, = Pj. 
下 面 说 明 上 述 和 迭代 过 程 是 收敛 的 。 首 先 由 式 〈4-41) W, HFE Pjs Prj M 
Py; EB lim K, *0, 则 根据 式 (4-41) 可 知 对 任意 了 =12,…< oo 都 有 P, ; «Pia i 
成 立 。 同 时 ， 由 于 矩阵 及， 中 的 每 一 个 元 素 是 有 界 的 ， 由 此 得 〈 见 附录 了 证明 ) 
lim P, = lim P, ia (4-43) 
显然 在 式 〈4-43) 成 立 的 前 担 下， 由 式 (4-41) 可 推 知 lim Kj =0， 这 与 前 面 的 假 
设 条 件 lim K, =0 相 矛盾 。 故 原来 的 假设 不 成 立 ， 唯一 的 可 能 是 lim K, =0。 现 假定 当 


j> NW K, , 一 0, 则 由 式 (4-40) 和 式 (4-41) 可 得 总， 一 Šik- = $r a 有 P,P, 
Dakika T SAP E E SK hi, 

根据 式 (4-40) 可 知 ， 第 N 次 迭代 的 结果 为 wy = Xu t0 Er T ax). 
ERRAT? 的 取 值 为 0<7<1 ， 则 当 TEA XB, BF 20, HX. 
-LN = 妃 ,w-1。 由 此 同样 可 知 ， 迭 代 过 程 必然 会 收敛 到 某 一 解 ， 且 收敛 速度 受 7 的 制约 。 

与 传统 的 IEKF 算法 相 比 ，IUKEF 在 三 个 方面 有 明显 的 区 别 : 首先 在 步骤 6 中 ， 式 
(4-42) 确保 了 和 迭代 过 程 朝 似 然 增加 的 方向 进行 ， 即 迭代 过 程 和 逼近 最 大 似 然 意义 下 的 最 
优 解 《有关 证 明 见 附录 C)， 其 次 是 步 长 调整 策略 ， 衰 减 因子 7 (O«n S1) 用 于 削弱 
新 近 一 次 观测 对 预测 状态 〈 总-1, ) 的 修正 影响 ， 这 将 使 得 相 邻 两 次 迭代 结果 CX, 和 
x,j-1 ) 随和 迭代 的 进行 越 来 越 接 近 ， 从 而 加 速 收敛 过 程 ; 最 后 ， 在 每 一 次 欠 代 中 ， 新 的 样 
点 通过 式 (4-33) 得 到 ， 这 意味 着 样 点 的 生成 是 由 最 近 一 次 更 新 的 协 方差 矩阵 来 引导 的 ， 
使 得 样 点 逐步 向 真实 的 状态 收敛 。 

就 算法 复杂 度 而 言 ， 由 文献 [19] 可 知 对 于 维 数 为 工 的 状态 估计 问题 ，UKF 的 算法 复 
杂 度 与 EKF 4824, HAOR). Xiii, EER, FHT N 次 迭代 ， 则 IUKEF 的 
复杂 度 约 为 OOMNZ ) 。 由 此 可 见 ， 只 要 N 不 过 大 ，IUKF 运算 量 与 UKF 基本 在 同一 量 级 ， 
实现 起 来 并 不 困难 。 


© 对 于 正定 矩阵 A, Be R” ， 记 A< B ， 其 含义 是 对 任意 矢量 aeR" 且 az*0， 则 有 ar4a<a' Ba 。 
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44 简化 UKF 算法 


在 标准 的 UKF 算法 中 , 通常 将 系统 状态 、 过 程 噪声 和 观测 噪声 扩展 成 新 的 状态 量 来 
处 理 ， 这 在 维 数 较 高 的 情况 下 算法 复杂 度 将 会 显著 增加 ; 此 外 ,标准 的 UKF 算法 同时 将 
UT 应 用 于 状态 方程 和 观测 方程 ， 不 管 它们 是 否 为 线性 。 然 而 ， 在 大 多 数 应 用 中 ， 品 声 
是 加 性 的 ， 并 且 状 态 方程 和 观测 方程 并 不 都 是 非 线性 的 ， 对 这 类 系统 ， 可 以 在 不 影响 滤 
波 器 性 能 的 前 提 下 对 UKF 算法 进行 简化 、 降 低 其 运算 复杂 度 。 本 节 主 要 针对 目标 跟踪 的 
实际 应 用 背景 ， 根 据 系统 的 特点 提出 一 种 简化 的 UKF (SUKF) 算法 ， 以 降低 运算 开销 ， 
便于 实时 应 用 。 

假设 状态 量 、 过 程 噪声 和 观测 噪声 的 维 数 分 别 为 到 、 五 ,和 到 ， 则 增 广 后 得 到 的 状 
态 维 数 为 L = L. +L, +L, EADS 2L +1， 算 法 的 复杂 度 为 O(Z.) 。 相 比 之 下 ， 经 
典 EKF 的 算法 复杂 度 为 O(Z.) 。 显 然 ， 标 准 UKF 算法 的 复杂 度 与 增 广 状 态 量 的 维 数 呈 
ZKT, KL. LAL, 的 维 数 都 比较 高 时 ， 运 算 量 的 增加 非常 显著 ， 这 对 实时 应 
用 是 不 利 的 。 

事实 上 ， 对 于 大 多 数 应 用 背景 ， 都 具有 这 样 的 特点 : 四 过 程 噪声 和 观测 噪声 至 少 有 
一 项 是 加 性 的 ，@ 式 (4-16) 和 式 (4-17) 并 非 都 是 非 线 性 的 。 对 于 这 一 类 问题 ， 可 以 
在 保证 滤波 器 性 能 的 基础 上 对 标准 的 UKF 算法 进行 简化 ， 从 而 达到 降低 运算 量 的 目的 。 
此 时 ， 对 于 加 性 噪声 ， 无 需 对 状态 量 进行 增 广 ， 可 以 不 同 程度 地 减 小 运算 量 ， 而 对 于 线 
性 方程 , 理论 上 KF 是 最 优 的 , 因此 可 以 直接 用 KF 进行 运算 , 而 不 必用 多 个 样 点 来 近似 。 
为 表述 方便 ， 这 里 把 这 一 类 系统 所 对 应 的 UKF 算法 称 为 SUKF (Simplified Unscented 
Kalman Filtering) 算法]。 

对 于 式 (4-16) 和 式 (4-17) 所 描述 的 系统 ， 根 据 前 面 的 讨论 ， 可 以 分 成 表 4.1 所 列 
的 几 种 不 同 的 形式 。 显 然 ， 形 式 @ 最 为 复杂 ， 对 应 于 标准 的 UKF 算法 ; 而 形式 @ 最 为 简 
单 ， 退 化 为 KF 算法 ， 其 他 几 种 形式 都 可 以 用 SUKF 算法 来 处 理 。 


表 4.1 几 种 不 同 的 系统 方程 形式 


u 





形式 @) 所 描述 的 一 类 重要 系统 为 


= Fx, +w, (4-44) 

y, = h(x,)*v, (4-45) 

在 这 种 条 件 下 ， 状 态 方程 是 线性 的 ， 而 观测 方程 是 非 线性 的 ， 且 两 种 噪声 都 是 加 性 

的 高 斯 噪声 。 由 前 面 的 讨论 知 ， 用 SUKEF 算法 进行 处 理 时 ， 不 对 状态 量 进行 扩展 ， 同 时 
对 线性 部 分 直接 应 用 KF 进行 计算 ， 由 此 得 到 的 SUKF 算法 可 表述 如 下 。 
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Alg (4.4): Simplified Unscented Kalman Filtering 
步骤 1: 初始 化 
&, = Efe] (4-46) 
P, = E[(x, — X,(x, — X,)] (4-47) 
步骤 2: 计算 预测 值 和 方差 
对 于 ke 山 …, eo), # 
Ki-1 = FR, (4-48) 
P. = FP, F! +GQ,G" (4-49) 
步骤 3: 时 间 更 新 
x? -[&, | Saa + HB, Spa- KP. (4-50) 


2L : 
Pia = > ui" hx?) (4-51) 
i=0 
式 中 : i e€ 0,1,--.,2L , L 为 状态 量 维 数 。 
步骤 4: 测量 更 新 
AL 
P, => wi? [h(x?) - 9,4 AQ?) Pr] +R, (4-52) 
i-0 
2L au 
P = sO [x —2, JMD) p. T (4-53) 
i=0 
X, = Šik + | M ss (Yk — 3i) (4-54) 
P =P, -BPE (4-55) 


步骤 5: 重复 步骤 2 一 步骤 4。 

以 上 即 为 线性 状态 空间 、 非 线性 观测 模型 对 应 的 SUKF 算法 的 流程 ， 可 广泛 应 用 于 
一 般 的 目标 跟踪 应 用 场合 。 对 于 其 他 方面 的 应 用 ， 可 根据 实际 的 条 件 和 背景 得 到 相应 的 
算法 。 


45 ”联合 估计 UKF 算法 


前 面 讨论 的 是 UKF 用 于 状态 估计 的 问题 ， 而 状态 量 本 身 是 一 个 时 变 的 随机 变量 。 在 

实际 应 用 中 ， 通 常会 碰 到 下 式 所 描述 的 问题 ， 即 

x, = f(x, ,,u) + W, (4-56) 

y, = h(x,) + v, (4-57) 
即 状态 变量 x, 中 包含 某 一 未 知 的 确定 性 常量 uw 〈 称 为 参量 或 参数 )， 该 常量 可 能 是 匀 加 
速 目 标 中 的 加 速度 输入 、 转 弯 机 动 中 的 角速度 ， 或 者 其 他 类 似 的 参量 。 此 时 ， 只 有 得 到 
状态 量 和 参量 的 完整 信息 才能 获得 准确 的 状态 估计 。 在 1.3.3 节 介 绍 线性 系统 中 的 滤波 问 
题 时 已 经 提 到 , 在 解决 这 类 问题 时 , 通常 有 两 种 途径 , 分 别称 为 双重 估计 (Dual Estimation) 
和 联合 估计 (Joint Estimation)。 前 者 利用 两 次 滤波 估计 ， 第 一 次 估计 状态 变量 ， 第 二 次 
估计 未 知 参 数 ， 而 后 者 则 是 将 未 知 参数 与 状态 变量 扩展 成 新 的 状态 空间 x? =[x, uL]; 
并 利用 滤波 算法 同时 估计 求解 。 
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由 于 联合 估计 将 参数 和 状态 合成 为 一 个 扩展 状态 量 ， 因 此 能 对 参数 和 状态 之 间 的 看 
合 量 (这 里 指 互 协 方差 ) 


P. P 


M Xk Wk Mk 


T Fw E 

E[(xz — Elx; Gg — Elxz ]) ] = (4-58) 
进行 准确 建 模 ， 即 式 〈4-58) 考虑 了 状态 和 参数 间 的 相关 信息 。 而 对 于 双重 估计 ， 由 于 
将 参数 和 状态 分 开 来 独立 考虑 ， 在 考虑 某 个 量 时 将 另外 一 个 量 视 为 确定 的 常量 〈 非 随机 
变量 )， 即 隐 含 地 将 上 式 中 副 对 角 线 上 的 元 素 视 为 零 来 处 理 。 由 于 联合 估计 更 接近 于 真实 
的 模型 ， 因 此 能 得 到 更 好 的 性 能 。 

基于 这 样 的 推理 ， 得 到 的 JEUKF 算法 回流 程 可 概括 如 下 。 

Alg (4.5): Joint Estimation Unscented Kalman Filtering 


步骤 1: 初始 化 
&, = Elx] (4-59) 
P, = E((x, — X9)(x, — X). ] (4-60) 
步骤 2: 计算 样 点 
Ao,k-i = $i» wo = K /(L + x) 
Zia = ia tO), w =05KL+x=) (4-61) 


Xara = Xka QU X)P,),, War =0.5/(L+ x) 
式 中 : i=1, 2, L ° 了 为 扩展 状态 量 x 的 维 数 ; 参量 x 用 于 控制 协 方差 阵 。 
步骤 3: 时 间 更 新 


Aia = Fnac) (4-62) 
2L 
XQ = Y» W; Xi kk (4-63) 
i-0 
2L : "e 
Pi = 2 tu,[Z gea 7 Xia Lx, na =X] +O (4-64) 
i0 
Xa = Praga) (4-65) 
2L 
ua = ? mobs (4-66) 
i-0 
步骤 4: 测量 更 新 
2L 
B5 = 2," [3L ui 7 Jig al] 7 Jia I +R, (4-67) 
2L . : | 
Py = > VIZ aa 7 Xa laa 7 Jii] (4-68) 
X, = $ip K,(y, — Pipi) (4-69) 
P, zB, -KPK (4-70) 
K,-P,P,, (4-71) 
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46 应 用 实例 


4.6.1 被 动 目 标 跟 踪 
考虑 x-y 二 维 平面 内 被 动 跟踪 问题 ， 目 标 状态 方程 为 


x,,, 7 Fx, + Gw, (4-72) 
1 T 0 0 0572 0 
0 1 0 0 T 0 x i 
moe 0017| pin 0 osr x, =[x, X, X, k] s We =[w, wI 为 
0 00 1 0 T 


扰动 噪声 ， 且 E[w,]=0, cov[w,]= Q,ó, 。 
观测 量 包 括 角度 只 、 角 度 变化 率 图 和 频率 变化 率 f, ， 观 测 方程 表示 为 


arctan| — 
Yk 





9, + y, -x y Vok 
yi = @, = —— + Vk E h(x,) 十 Vi (4-73) 
f Xk + Yk » 
dk Jak 


_ (Xk 7X4 y 
AQ + Yi). 


AP: 4 为 信号 波长 ; wx 、vpx 和 vi 为 观测 噪声 且 相 互 独立 ， 其 方差 阵 
R, = diag(o, 05,0; ) : 


假定 观测 站 位 于 坐标 原点 ， 目 标 在 平面 内 作 近 匀速 运动 ， 其 初始 状态 为 


xy =[120km — 250m/s 80km 100m/s]" (4-74) 
目标 的 初始 位 置 估计 由 测 距 公式 
rzAf,! ff (4-75) 


综合 角度 信息 得 到 。 

在 仿真 中 ， 过 程 噪声 协 方差 矩阵 Q, = diag((3m/s2)2,(2m/s2)2) 。 对 不 同 观测 精度 
({2e-3rad, 1le—4rad/s, 0.5Hz/s}; (5e-3rad, 2e—4rad/s, 1Hz/s); (0.01rad, 5e—4rad/s, 2Hz/s} ) 
条 件 下 的 算法 性 能 进行 测试 ， 性 能 指标 用 相对 距离 误差 (RRE) 来 测度 ， 其 定义 为 

(xn -£y *t Um -fý 
Xen t Fen 
AP: Quae Yee) 为 真实 目标 位 置 ，(%, 攻 表示 位 置 估 值 。 

在 每 一 组 仿真 中 取 100 次 蒙特 卡 罗 实 验 , 其 结果 见 表 4.2”。 由 表 中 的 结果 可 以 看 出 ， 

在 高 精度 观测 条 件 下 ， 几 种 跟踪 算法 都 能 较 好 地 收敛 。 但 是 ， 随 着 测量 误差 的 增 大 ， 各 


RRE = x100% (4-76) 


© 在 仿真 1~ 仿 真 3 中 , 测量 误差 分 别 为 {2e-3rad, le-4rad/s, 0.5Hz/s}, (5e—rad, 2e-4rad/s, 1Hz/s} 和 {1e-2rad, 5e-4rad/s, 
2Hz/s}; 在 每 次 试验 中 ， 跟 踪 时 间 为 120s; 若 在 跟踪 结束 时 刻 RRE <15% 则 本 次 跟踪 视 为 收敛 ， 否 则 视 为 发 散 ，IUKF 
和 IEKF; 采 用 同一 迭代 策略 , 其 最 大 和 迭代 次 数 N=5, 衰减 因子 w=0.85; 而 IEKF; 则 采用 传统 迭代 策略 , 其 门限 为 Vw=100。 
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种 算法 的 性 能 恶化 程度 却 大 不 相同 。 相 比 之 下 ，EKF 算法 由 于 线性 化 的 影响 对 测量 误差 
更 加 敏感 。 从 表 中 可 以 看 出 ， 迭 代 型 EKF CEKF,; 和 IEKF,) 的 性 能 很 大 程度 上 受 迭 代 
策略 的 影响 , 在 收敛 性 准则 选择 合适 的 情况 下 IEKF; 总 能 比 EKF 和 MVEKF 得 到 更 好 的 
估计 性 能 ， 这 一 点 与 前 面 的 理论 分 析 是 一 致 的 。 这 是 因为 ， 本 质 上 EKF 和 MVEKEF Hk 
代 型 EKF 的 两 种 特殊 形式 ， 对 应 于 无 迭代 和 1 次 迭代 。 同 样 ， 当 迭代 门限 选择 不 当时 ， 
可 能 会 导致 算法 性 能 严重 恶化 ， 如 表 中 的 EKF. 


表 4.2 跟踪 算法 的 鲁 棒 性 比较 


100 次 试验 中 跟踪 收敛 的 次 数 





仿真 中 发 现 ， 文 中 的 收敛 性 准则 一 般 经 2 次 ~3 次 就 能 满足 ， 这 意味 着 IUKF 的 平 
均 运 算 开 销 约 为 UKF 的 2 倍 ~3 倍 。 

在 仿真 中 ,参数 上 默认 地 设 为 0, 但 实际 上 只 要 保证 所 生成 的 样 点 有 良好 的 数值 稳 
定性 ( 权 值 不 为 负数 )， 算 法 的 性 能 对 该 值 并 不 敏感 ， 同 样 7 的 选择 也 可 比较 随意 ， 
当 0.5<7 乏 1， 对 算法 的 性 能 影响 不 大 ， 但 若 7 选 择 不 当 〈 如 ?>1) 可 能 会 导致 滤波 器 
不 稳定 。 

为 了 提供 更 直观 的 性 能 比较 ， 图 4.2 示 出 了 中 等 观测 精度 (仿真 2) 条 件 下 几 种 算法 
的 平均 RRE。 需 要 指出 的 是 ， 图 4.2 中 的 结果 反映 的 是 蒙特 卡 罗 试验 的 统计 性 能 ， 且 已 
剔除 了 跟踪 发 散 的 试验 (RRE >15% )。 

如 图 4.2 所 示 ，UKF 无 论 在 收敛 时 间 还 是 跟踪 误差 方面 都 要 优 于 EKF， 在 统计 平均 
的 意义 下 MVEKF 看 似 比 UKF 跟踪 误差 要 小 ， 但 不 能 简单 断言 MVEKF 的 性 能 要 优 于 
UKF， 这 是 因为 ， 在 同一 组 仿真 中 MVEKF 剔除 了 更 多 不 收敛 的 试验 结果 。 从 图 中 可 以 
看 出 ， 与 非 欠 代 型 滤波 器 相 比 ， 迭 代 型 滤波 器 通常 有 更 快 的 收敛 速度 ， 这 一 特点 使 得 迭 
代 型 滤波 器 更 具 优 势 ， 特 别 在 初始 误差 较 大 的 情况 下 更 是 如 此 。 很 明显 ，IUKF 无 论 在 
收敛 速度 和 跟踪 精度 上 都 要 优 于 其 他 几 种 算法 (EKF, IEKF'1, IEKF;, UKF 和 MVEKF), 
尽管 这 些 算 法 已 经 剔除 了 不 收敛 的 跟踪 结果 。 

在 实际 应 用 中 ,通常 更 关心 的 是 单 次 试验 中 算法 的 稳定 性 ， 从 这 一 意义 来 说 ，IUKF 
的 优势 更 加 明显 。 

为 进一步 分 析 所 提 IUKF 算法 的 稳定 性 ， 图 4.3 给 出 了 状态 估计 标准 偏差 和 跟踪 误 
差 的 比较 示例 。 不 同 坐 标 轴 的 跟踪 误差 定义 为 

€,—Xu 7i. ü. yu 7) (4-77) 

相应 的 标准 偏差 表示 为 
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o, = p: Do (4-78) 
式 中 :， 忆 表示 跟踪 误差 协 方 差 矩 阵 己 的 主 对 角 元 素 。 


一 一 一 一 EKF 


一 一 一 ,IEKF， 


Relative Range Error(%) 


Tracking Error Vs.Standard Deviation(m) 





0 20 40 60 80 100 120 
Time(s) 


图 4.3 跟踪 误差 和 标准 偏差 比较 


从 图 4.3 可 以 看 出 ， 当 滤波 器 收敛 之 后 ， 其 状态 估计 的 标准 偏差 与 跟踪 误差 能 较 好 
地 吻合 。 这 就 意味 着 ， 标 准 偏差 能 准确 描述 估计 误差 。 由 此 可 以 推 知 ， 一 旦 标准 偏差 达 
到 稳定 状态 ， 跟 踪 误差 也 相应 地 趋 于 稳定 〈 即 误差 不 会 有 大 的 起 伏 )。 

为 进一步 验证 算法 性 能 ， 将 IUKF 用 于 外 场 实测 数据 处 理 。 试 验 场景 如 图 4.4 所 示 ， 
载 有 雷达 信号 源 的 小 车 从 某 跑 道 的 一 端 运动 到 另 一 端 ， 观 测 平台 固定 不 动 ， 它 与 辐射 源 
运动 轨迹 的 垂直 距离 约 为 520m, 通过 雷达 接收 信号 、 参 数 估计 等 手段 获得 目标 辐射 源 的 
波 达 角 、 角 度 变化 率 和 频率 变化 率 ， 综 合 利用 2 、9 和 广 进 行 定位 与 跟踪 。 在 试验 中 ， 
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采用 差分 GPS 提供 的 高 精度 位 置信 息 作 为 定位 与 跟踪 结果 的 参考 数据 。 





图 4.4 目标 一 观测 器 相对 几何 示意 


试验 中 ， 辐 射 源 以 约 15/m 的 速度 从 右 向 左 运动 ， 统 计 结 果 表 明 其 参数 测量 精度 为 
(0.36mrad, 2.5mrad/s, 2.8Hz/s) 。 采 用 几 种 不 同 算法 (EKF，IEKF,，IEKF2，UKF，IUKF 
和 MVEKF) 分 别 对 目标 进行 跟踪 ， 其 结果 如 图 4.5 和 图 4.6 所 示 。 

由 图 4.5 和 图 4.6 可 以 清楚 地 看 出 ， 迭 代 型 滤波 器 (IEKF1，IEKF, 和 IUKF) 能 比 非 
友 代 型 滤波 器 (EKF 和 UKF) 对 新 的 观测 做 出 更 快 的 反应 ， 也 正 因为 此 ， 在 测量 精度 足 
够 高 的 情况 下 ， 和 迭代 型 滤波 器 的 收敛 速度 也 相对 较 快 。 很 明显 ，UKF 和 MVEKF 都 比 经 
典 EKF 有 更 好 的 性 能 ， 这 是 因为 这 两 种 算法 对 状态 均值 和 协 方差 的 近似 精度 比 EKF 要 
高 。 试 验 结果 同样 表明 ， 对 于 迭 代 型 EKF， 其 迭代 策略 对 结果 产生 重要 的 影响 ， 因 此 合 
适 的 终止 准则 和 门限 选择 是 提高 滤波 器 性 能 的 关键 。 

与 前 面 的 仿真 结果 一 致 的 是 ，IUKF 无 论 在 收敛 速度 还 是 跟踪 误差 方面 都 要 优 于 其 
他 几 种 算法 。 TUKF 的 这 一 显著 优势 主要 来 自 于 两 个 方面 : 一 是 UKF H EKF 的 估计 精度 
要 高 ， 这 一 点 在 仿真 和 试验 结果 中 都 得 以 体现 ， 二 是 因为 迭代 型 UKF 比 标准 UKF 有 着 
更 快 的 反应 速度 ， 从 而 能 够 缩短 收敛 时 间 。 
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H45 几 种 滤波 器 跟踪 结果 比较 


(D 本 试验 中 ，IUKF 和 IEKF; 采 用 同一 迭代 策略 且 N=5, m-0.8; IEKF, 采用 传统 迭代 算法 ， 终 止 门限 V4-50. 
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图 4.6 ”相对 距离 误差 比较 
4.6.2 ”转弯 目标 跟踪 


本 实例 主要 针对 目标 转弯 机 动 的 运动 特点 ， 提 出 并 验证 一 种 利用 单个 模型 结合 联合 
估计 UKF 算法 (JEUKF) 跟踪 机 动 目标 的 能 力 。 
考虑 二 维 平面 的 目标 运动 问题 ， 直 角 坐 标 系 下 匀速 运动 (CV) 目标 的 离散 状态 方程 
可 表示 为 
x, = Fy% + T w, (4-79) 
RP: x,=[x, 3 Y, 六 了 为 目标 状态 ，w =[w。w,,] 为 零 均 值 扰 动 噪声 ， 其 协 方差 矩 
EXO: ，7 为 采样 间隔 ， 且 有 


17 00 T'] 4€ 
Ó 1 0 D T 0 
F. = ,T= 
= í“ 6 tP ü 773 
000 1 0 T 


当 目标 做 转弯 机 动 时 ， 若 在 某 段 时 间 内 ， 转 弯 率 《角速度 ) 保持 恒定 不 变 ， 则 称 为 
EHS (CT) 运动 或 圆周 运动 。 此 时 ， 若 角速度 @ 已 知 ， 则 相应 的 运动 方程 为 


x, = Fx + Tw (4-80) 
式 中 
1 sin(oT) 0 cos(oT) -1 
o @ 
_|0 cos(@T) 0 —sin(oT) 
"i 0 1 - cos(oT) 1 sin(oT) 
o o 
0  sin(oT) 0  cos(oT) 


其 他 量 的 含义 与 式 (4-79) 相同 。 
显然 ， 对 于 非 合作 目标 ， 很 难 确 知 吧 的 先 验 信息 ， 故 只 能 将 中 作为 一 个 未 知 的 参数 
来 估计 。 此 时 ， 扩 展 的 状态 量 为 x =[x, Xx, y, h 由， 于 是 运动 模型 变 为 
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x, = F°x,_ + Iw, (4-81) 
式 中 
F^-diag(F,,v), T° =diag(T,ll, w,=[w, w, w] 

对 于 机 动 目标 跟踪 ， 常 用 的 方法 主要 有 输入 估计 〈IE)、 变 维 滤波 (VDF) 和 交互 多 
模 (MM) 等 。IE 算法 和 VDF 算法 需要 对 目标 机 动 进行 检测 ， 故 存在 检测 滞后 问题 ， 
跟踪 误差 较 大 ，IMM 算法 则 采用 若干 模型 并 行 工作 ， 虽 然 不 存在 机 动 检测 滞后 的 问题 ， 
但 由 于 需要 同时 运行 多 个 滤波 器 ， 增 加 了 运算 开销 ， 且 多 个 模型 之 间 的 竞争 会 降低 算法 
的 跟踪 性 能 。 有 关 多 模型 滤波 问题 将 在 第 12 章 进行 详细 阐述 。 

由 式 (4-81) 知 ， 要 对 目标 进行 有 效 跟 踪 ， 需 要 同时 对 目标 状态 〈 包 括 位 置 、 速 度 ) 和 
机 动 参数 (角速度 ) 进行 准确 估计 ， 属 典型 的 联合 估计 〈 正 ) 问题 。 同 时 注意 到 在 式 
(4-800 中 ， 当 @ 较 小 (w 一 0) 时 ， 有 


= Fy (4-82) 


显然 ， 式 (4-82) 正 是 匀速 运动 所 对 应 的 状态 转移 矩阵 。 由 此 可 知 ， 只 要 能 准确 地 
估计 出 角 速 率 吧 ， 就 可 以 用 单个 模型 跟踪 目标 的 运动 ， 与 文献 [1] 中 采用 CV 模型 和 CT 
模型 组 成 的 IMM 算法 相 比 ， 可 以 在 保证 跟踪 性 能 的 条 件 下 大 大 简化 算法 的 复杂 度 。 

仿真 中 假定 目标 初始 位 置 为 (xo, 加 ) = (45,60)km , 初始 速度 (如 ,为 ) =(120,160)m/s , x. 
y 轴 的 过 程 噪声 为 0.1m/s*，w 的 随机 游 动 误差 为 le-4rad/s，7T=2s 。 目 标 航 迹 分 为 5 Bt: 

(1) 0s 一 200s: 以 初始 速度 做 匀速 直线 运动 。 

(2) 200s 一 380s: 第 一 次 转弯 机 动 ，@%= 0.5deg/s 。 

(3) 380s 一 540s: 第 一 次 转弯 后 继续 做 匀速 直线 运动 。 

(4) 540s—630s: 第 二 次 转弯 机 动 ，w=1.0deg/s 。 

(5) 630s 一 900s: 第 二 次 转弯 后 继续 做 匀速 直线 运动 。 

采用 雷达 传感器 进行 观测 ， 斜 距 和 方位 观测 精度 分 别 为 10m 和 0.3deg。 初 始 目标 位 
置 和 速度 估计 误差 分 别 为 (35,30)m 和 (50,50)m/s , 跟踪 算法 采用 EKF 和 UKF, 蒙特 卡 罗 
仿真 次 数 为 500， 所 得 结果 如 图 4.7 一 图 4.10 所 示 。 

仿真 过 程 中 发 现 , 采用 单 模型 的 EKF 算法 在 第 一 次 机 动 发 生 时 就 开始 发 散 。 究 其 原 
因 ， 是 因为 在 线性 化 过 程 中 ， 由 于 角速度 和 速度 之 间 存 在 耦合 ， 使 得 速度 估计 的 误差 对 
角速度 的 估 值 产生 了 严重 影响 ， 从 而 偏离 真实 的 运动 模型 ， 误 差 的 积累 最 终 导 致 滤波 器 
发 散 。 相 反 ， 由 于 UKF 算法 在 滤波 过 程 中 不 涉及 函数 展开 ， 比 EKF 具有 更 高 的 精度 ， 
因此 能 对 目标 航 迹 进行 有 效 跟踪 。 

4.7 为 单 次 仿真 的 航 迹 跟踪 图 ， 图 4.8 为 跟踪 过 程 中 的 转弯 率 估计 结果 。 由 图 4.7 
和 图 4.8 中 可 以 看 出 , 采用 单 模 型 JEUKF 可 以 实现 对 转弯 目标 运动 轨迹 的 准确 跟踪 且 跟 
踪 过 程 较 为 稳定 ， 这 是 因为 跟踪 过 程 中 实现 了 对 转弯 率 的 有 效 估 计 。 从 图 4.8 的 结果 看 ， 
尽管 机 动 参 数 的 大 小 估计 较为 准确 ， 但 在 目标 运动 发 生 模式 变化 时 ， 转 弯 率 的 估计 存在 
一 定 的 滞后 现象 ， 这 是 因为 本 实例 中 给 出 的 转弯 率 是 阶 跃 型 的 ， 建 立 稳定 的 估计 需要 一 
定 的 时 间 积 累 造成 的 。 
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图 4.7 目标 航 迹 单 次 跟踪 图 4.8 目标 转弯 率 估 计 
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图 4.9 目标 距离 跟踪 均 方 根 误差 图 4.10 ” 转 谊 率 估 计 均 方 根 误差 


图 4.9 和 4.10 给 出 的 是 500 次 实验 得 到 目标 距离 和 转弯 率 估 计 均 方 根 误 差 ， 由 此 可 
以 看 出 ， 采 用 联合 估计 的 方法 ， 滤 波 器 能 对 目标 的 联合 状态 进行 准确 估计 ， 进 而 对 运动 
模式 变化 〈 对 应 机 动 的 开始 或 结束 ) 作出 快速 的 响应 。 机 动 完 成 后 ， 转 弯 率 的 估 值 也 迅 
速 降 至 0 附近 ，CT 模型 因此 退化 为 CV 模型 ， 这 正 是 采用 单 模 型 的 UKF 算法 实现 机 动 
目标 跟踪 的 原因 。 与 传统 的 具有 M 个 模型 的 IMM 算法 相 比 ， 单 模型 跟踪 算法 简单 、 高 
效 ， 其 运算 复杂 度 低 于 IMM 算法 的 UM. 


47 本 章 小 结 


从 贝 叶 斯 估计 的 观点 来 看 ， 滤 波 问题 的 关键 是 如 何在 给 定 观测 量 的 基础 上 建立 状态 
的 后 验 分 布 ， 从 而 求 得 状态 的 均值 和 方差 。 对 于 一 般 的 非 线性 方程 ， 很 难得 到 后 验 状 
态 分 布 的 解析 表达 式 ， 为 方便 处 理 ， 通 常 将 这 一 分 布 近似 为 高 斯 分 布 。 很 明显 ，EKF 
和 UKF 都 是 建立 在 解析 高 斯 近似 的 基础 上 ， 但 是 这 两 类 算法 的 实现 途径 不 同 ，EKF 及 
其 衍生 算法 通过 泰勒 展开 将 非 线性 方程 进行 线性 化 ; 而 UKF 则 利用 UT 方法 对 样 点 进行 
变换 。 由 于 基于 UT 的 方法 精度 高 且 方 便 实 现 ， 因 此 UKF 比 传统 EKF 具有 很 大 的 优势 。 
作为 对 比 , 本 章 首先 详细 讨论 了 UKF 算法 原理 和 实现 方法 , 在 此 基础 上 结合 一 种 新 的 和 迭 
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RMSE of range estimation(m) 
ə 
° 
RMSE of turn rate estimation(deg) 


代 策 略 提出 了 UKF BARS: 针对 一 些 特殊 的 系统 ， 为 了 降低 算法 运算 开销 ， 提 高 实 
时 性 ， 介 绍 了 UKF 的 简化 算法 一 一 SUKF; 考虑 到 含 参 量 的 系统 估计 问题 ， 还 研究 了 用 
于 状态 一 参数 联合 估计 的 JEUKF 算法 。 本章 最 后 通过 多 个 实例 对 前 文中 的 算法 进行 了 仿 
真 测试 和 实测 数据 验证 。 

从 跟踪 应 用 的 实例 结果 看 ，UKF 算法 在 性 能 方面 要 优 于 EKF 算法 ， 尤 其 是 在 初始 
误差 较 大 的 情况 下 更 是 如 此 。 这 是 因为 ， 当 初始 误差 较 大 时 ， 由 一 阶 线性 化 近似 导致 的 
误差 可 能 显著 增 大 ， 严 重 影响 滤波 器 的 性 能 。 另 外 ， 和 迭代 型 算法 在 收敛 速度 方面 要 优 于 
非 迭 代 型 算法 ， 这 是 因为 在 滤波 过 程 中 进行 了 多 次 修正 的 结果 。 作 为 一 种 较 新 的 非 线性 
滤波 算法 ，UKEF 因 其 近似 精度 不 受 非 线性 程度 的 影响 ， 且 计算 较为 方便 等 特点 ， 在 实际 
应 用 中 受到 越 来 越 多 的 关注 。 
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第 S$ 章 求 积 近似 高 斯 滤波 


通过 第 3 章 和 第 4 章 的 介绍 不 难看 出 ， 函 数 近似 高 斯 滤波 和 确定 采样 近似 高 斯 滤波 
都 是 最 优 贝 叶 斯 滤波 在 非 线性 条 件 下 的 两 种 次 优 近 似 ， 两 者 既 有 区 别 又 有 联系 。 前 者 采 
用 的 是 直接 对 非 线 性 函数 本 身 进行 近似 ， 而 后 者 则 是 利用 有 限 个 确定 的 采样 点 对 系统 的 
分 布 进行 近似 ， 但 两 者 都 将 状态 量 视 为 GRV， 以 此 为 前 提 得 到 了 卡尔 曼 滤 波 框架 下 的 各 
种 估计 算法 。 

事实 上 ， 从 第 1 章 贝 叶 斯 滤波 的 通用 框架 中 可 以 清楚 地 看 出 ， 滤 波 过 程 中 涉及 的 两 
个 重要 步骤 ， 即 预测 和 更 新 ， 包 含 了 两 个 积分 运算 。 在 高 斯 假设 条 件 下 〈 即 预测 分 布 和 
后 验 分 布 都 近似 为 高 斯 分 布 ) ， 该 积分 式 等 效 为 高 斯 函数 与 非 线 性 函数 乘积 的 多 维 积分 
运算 ， 这 一 特点 给 近似 高 斯 滤波 的 实现 提供 了 一 个 新 的 思路 ， 各 种 求 积 近似 滤波 方法 就 
是 通过 该 积分 式 的 数值 近似 来 实现 的 。 

本 章 主要 阐述 基于 求 积 近似 的 高 斯 滤波 原理 和 应 用 。 首 先 给 出 了 一 般 非 线性 方程 的 
高 斯 一 厄 米 特 求 积 计 算 原 理 ， 由 此 得 到 了 基于 这 一 计算 原理 的 求 积 卡尔 曼 滤波 COKF) 
算法 ， 考 虑 到 传统 求 积 计算 中 求 积 点 个 数 随 状 态 维 数 的 增加 呈 指 数 增长 关系 可 能 导致 的 
维 数 灾难 问题 ， 本 章 引 入 了 一 种 运算 相对 简单 的 求 容积 规则 ， 并 得 到 了 求 容 积 卡尔 曼 滤 
波 〈CKF) 算法 及 其 求 根 实现 形式 ， 最 后 给 出 了 目标 跟踪 的 应 用 实例 。 
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5.1.1 GH 求 积分 规则 
考虑 下 面 的 单 变量 函数 求 积分 问题 
I(g) = Ju(x)g(x)dx 
式 中 : w(x) 为 权 值 函数 ， 满 足 非 负 性 ， 取 严 个 积分 点 ， 可 近似 得 到 
IG Yu) 


AP: z 为 第 7 个 求 积分 点 ; w 为 其 权 值 。 
假如 给 定 痉 个 积分 点 ， 权 值 的 计算 可 首先 通过 计算 其 上 阶 窍 M, 


b 
M, = f. x'w(x)dx, i20,1--.,m-1 


进而 求解 下 式 所 示 的 矩 方程 得 到 
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A^ Xa An wW - M, 
2A Elo mE 
E X =E AS Ma 


若 采 用 Gauss-Hermite 求 积 规则 ， 则 权 值 函数 取 为 标准 正 态 分 布 , 假设 w(x)=N (x;0,1) ， 
此 时 有 


E[g(x)] = [gw (x;0,1)dx = wgl) 
j=1 
3b, PAAL 的 一 种 选取 方法 为 : 构造 一 个 对 角 对 称 和 矩阵 J ， 对 角 上 的 元 素 为 0， 
J. =Vi12(i=1,2,…,m 一 1) ; Sr z, 724, Àj AERJ (028 ANE, BUE 


w, =P, (v) 93626; B MU EHEEI EIS — 176. 
以 最 常用 的 m=3 点 求 积 公式 为 例 ， 根 据 上 述 规则 构造 矩阵 


0 2⁄2 0 
J«l4203 6 1 
0 1 0 


由 此 得 到 的 特征 值 为 4=0,- V3/2,V3/2 ， 对 应 的 归 一 化 向 量 分 别 为 
v=[—V2/3,0,1V3T 
v,=[-1/46, V2/2, -V3T 
v,=[1/ V6, V2/2, V3T 


最 终 可 得 3 个 积分 点 为 x =0,+V3 ， 相 应 地 ， 得 w=2/3,1/6,1/6。 
对 于 n, 维 矢量 x~~ 和 N(x;0,1,)， 其 高 斯 - 厄 米 特 求 积 规则 如 下 


Elg(x)]= [860 Gc 6,1, Xx 


m m m"x (5-1 ) 
“> t D wga) = 2,u,g(z;) 
me jl ja 


RP: xj =la otn TR, = ] Duo, 分 别 表示 第 7 个 求 积分 点 和 其 所 对 应 的 权 值 。 
p=l 
进一步 假设 x~ N(x, P), 32 P=SST, y=S' (x-2), f8 
Elg(x)]= |EN Gc $, Pix = [g(Sy + 3) (r0, 1, dy 


= D, wp, cw EUG En ts Y +) 
A71 


n, =l 


(5-2) 


= Y w,g(Sx, +) 
jal 
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5.1.2 GH 求 积 卡尔 曼 滤 波 
重新 考虑 式 〈3-13) 和 式 〈3-14) 所 述 的 非 线性 系统 
x, = f(x, ,) + W, (5-3) 
z, = h(x,) + v, (5-4) 

式 中 ， 各 变量 的 含义 与 前 文 相同 。 

在 卡尔 曼 滤 波 框 架 下 ， 引 入 高 斯 - 厄 米 特 求 积 分 规则 计算 高 斯 状态 经 非 线性 系统 传 
递 后 的 数值 积分 解 ， 可 得 求 积 分 卡尔 曼 滤波 〈QKF) 算法 请。 下 面 将 具体 分 析 QKF 算 
法 两 个 重要 步骤 一 一 时 间 更 新 和 观测 更 新 的 计算 过 程 。 

1. 时 间 更 新 

假设 已 知 k-l 时 刻 的 概率 分 布 p(X alza) = N(x, ;1, PP) ， 且 过 程 品 
p(w, 1) = N(w;0,0,..), £ P,,- S, SL. HK (5-2) TA, RAM 点 求 积 公式 时 ， 
一 1 时刻 第 j 个 (j=1,2,…,m” ) 求 积 点 可 表示 为 

Ck = Se X; * Xa (5-5) 

4 M-m' ， 结 合式 (1-13) 和 式 (1-14) 可 得 大 时 刻 状 态 的 预测 值 ea 及 其 协 方差 

Py 分 别 为 


M 
Xa L f (XWN (x; &, ,, P, )dx, = 2, (čik) (5-6) 
Ja 


Pl = L. [f (x, .,)*w, 一 X, ak Jf (x, ,)*w, 一 X, ak FN (x, ,;X, ,, P, ,)dx, , 


a L. fo f (xe 0N 09 5:3, 4, P, dx, - £, aa +Q; (5-7) 


= >u, f (6; ka) f T(E jka)— 5 ax +Q; 
j=l 
2， 量 测 更 新 
假设 预测 概率 分 布 和 观测 噪声 分 布 可 分 别 表示 为 P(Xi |z, a) =N (Xk kip Pa) 和 
p(v,)2N(;9,R), H Piri = Siw1S4w1， 则 根据 预测 分 布 得 到 的 第 j 个 求 积 分 点 为 


Č ji = Sky ` Xj Xa , j=1,2,…,M (5-8) 
类 似 地 ， 有 
M 
Za L. h(x, )N (x, ; X, ,, P, .,)dx, = > u, h(¿ jk) (5-9) 
Jl 


P zkkl = i h(x, y (x, )N (x; Xy. Paca )dx, 


3 1 s ox (5-10) 
š >u, h(@ jp- )h (¿ j,klk-l )— kkrk- t R, 
jal 
Pu m x,h(x, )N Q5, a Pa )dx, 一 itii 
(5-11) 


M 
T ^ T 
z $ we, ih (£i ) 一 Xacakk-i 
jzl 


由 此 ， 结 合 前 文 介绍 的 卡尔 曼 增 益 及 协 方差 矩阵 计算 公式 ， 即 可 得 到 QKF 算法 , 具 
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体 流程 如 可 概括 如 下 。 
Alg (5.1): Quadrature Kalman Filtering 
步骤 1: 初始 化 
给 定 均值 和 协 方差 如 = Elx], P = cov(x,) ; 
WHERE ja 2,0712, m) 及 相应 的 权 值 ww) 。 
步骤 2: 时 间 更 新 
(1) 滤波 协 方差 矩阵 分 解 ， 即 
R- = Spaski 
(2) 计算 求 积 点 ， 即 
Šik- = Ski X; 十 X,1 
(3) 传播 求 积 分 点 ， 即 
č jkk-1 7 JJ jk) 
(4) 状态 预测 ， 即 


" M 
Xkk-1 = 2 vana 
j= 
(5) 预测 误差 协 方 差 ， 即 


M 
A A T 
Pe = >u; (€ a 7 Fre- Ejk- 7 Xii). + Qa 
jzl 


M 

— T ^ ^T 

=>, W € kk; eje 一 Xkk-1¥kk-1 十 Q; 
jal 


步骤 3: 量 测 更 新 
(1) 预测 协 方差 矩阵 分 解 ， 即 
Pik- a M E 
(2) 计算 求 积 分 点 ， 即 
č jkik-1 = Ska Xj+ Xa 
(3) 求 积 点 传播 ， 即 
EL= Ga) 
(4) 观测 的 预测 ， 即 


M 
Zik-1 = 2 wh ai) 
jal 
(5) 预测 误差 协 方差 ， 即 


M 
P. i >u, (E; xk- T Ln XE; s-i E Zac y +R, 
j=1 


M 

= T ^ ^T 

p we, ae ea 7 fea mea tR 
ja 


(6) 计算 互 协 方差 矩阵 ， 即 
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M 
^ ^ T 
Pa = >u, (Š; kk- — Xp XE; kk- gs Za) 
j=l 


M 
as T ^ ^T 
gs wi S jag 一 Xkk-1Zkk-1 
j=l 


CD 计算 卡尔 曼 增 益 ， 即 
K, = P, P-' 
(8) 状态 更 新 ， 即 
X, = Xa + K, (z, — 2,43) 


(9) 状态 协 方差 矩阵 更 新 ， 即 
P, = Pr -K,P.K, 


52 求 容积 卡尔 曼 滤 波 


通过 5.1 节 的 介绍 可 知 ， 在 利用 高 斯 - 厄 米 特 求 积 规则 进行 积分 式 近似 计算 时 ， 其 算 
法 复杂 度 与 状态 量 的 维 数 及 在 每 一 维度 上 求 积 点 的 个 数 密切 相关 。 对 于 nn 维 状态 量 ， 若 
求 积 点 数 为 m ， 则 根据 式 (5-1) 易 知 总 的 求 和 项 为 m". 。 求 积 点 的 个 数 决定 了 近似 精度 ， 
m 个 求 积 点 对 应 2m 一 1 阶 多 项 式 精度 。 在 m 确定 的 条 件 下 ， 函数 的 计算 次 数 随 维 数 nn, 呈 
指数 增长 ， 即 存在 “ 维 数 灾难 ”问题 ， 因 此 这 种 方法 一 般 只 能 适用 于 维 数 不 太 高 (如 小 
于 6) 的 场合 。 

对 于 高 维 函 数 的 求 积 运算 ， 有 必要 需求 新 的 解决 途径 。 为 此 ， 文 献 [29] 中 提出 了 一 
种 具有 线性 复杂 度 且 性 能 优良 的 求 容积 方法 ， 并 由 此 导出 求 容积 卡尔 曼 滤波 (CKF) 算 
法 ， 下 面 对 此 进行 详细 介绍 。 


5.2.1 求 容积 规则 
考虑 下 面 的 求 积分 问题 
If)». | f(x)exp(-x" x)dx (5-12) 
首先 ， 将 其 变换 为 球 径 (Spherical-Radial) 求 容积 的 形式 ， 然 后 运用 3 度 的 球 径 求 容积 
规则 进行 求解 。 具 体 求 解 方法 如 下 。 
BEXEN, y'y=1, 得 xIx= 产 ， re[0,co) , 式 (5-12) 的 积分 可 在 球 径 坐 标 系 
中 表示 为 
IP= f, fu, fev" exp(-r’)d(oy)dr (5-13) 
RP: U, 表示 半径 为 1 的 球 表面 ， 即 U，= {ye R™ | =] ，a() 表 示 球 面 测度 。 
进一步 化 简 ， 式 (5-13) 可 表示 为 
Kf)» Ë S(r)r"^ exp(—r2 dr (5-14) 
S()= f, /fG)do(y) (5-15) 
对 式 (5-14) MA (5-15) 分 别 采 用 m, 点 的 径 (Radial) 规则 (用 高 斯 - 厄 米 特 求 积 
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实现 ) 和 mm 点 的 球 (Spherical 规则 ， 可 得 m, xm, 点 的 球 径 求 容积 规则 为 
| SC) expr’ )dr = 50) 


f. fe) = 2b, 09) 


I(f)- | f(x)exp(-x" x)dx = >a ab, f(ny;) 


Him, =1 和 m, =2n,， 可 得 3 度 的 球 径 求 容积 规则 。 
根据 上 述 的 求 容 积 规则 ，n, 维 标准 正 态 分 布 与 非 线 性 函数 乘积 的 积分 可 近似 为 


IU)» 人 £00 VGa0,1, Jd = Sw f(x) 


RP: m=2n,, z, = Vml2]]): w-Vm, G=12,...m); (eR, H 
0 0) /-11 í 0 0 


[1], € d : 3t eppo bb ez "T : 
0/10 l 0 0 -1 
类 似 于 求 积 规则 ， 假 设 x~ NRP), P-S, & 
I() 7 |, SONE, Pax = Yu, f (Sg, + $) 
i=l 


5.2.2 求 容积 卡尔 曼 滤 波 


求 容积 卡尔 曼 滤波 的 形式 与 求 积 分 卡尔 曼 滤波 相同 ， 仅 用 求 容积 点 代替 其 中 的 求 积 


分 点 而 得 到 ， 具 体 流 程 可 概括 如 下 。 
Alg (5.2): Cubature Kalman Filtering 
步骤 1: 初始 化 
给 定 均值 和 协 方差 名 =E[xo]，P =cov(x,) ; 
计算 求 容积 点 Xj (j=1,2,- z m) 及 相应 的 权 值 wi, 
步骤 2: 时 间 更 新 
(OD 滤波 协 方 差 矩 阵 分 解 ， 即 
Ha SS 
(2) 计算 求 积 点 ， 即 
$a = Sia X; + Xa 
(3) 传播 求 积分 点 ， 即 
š jkk- = f (6; a) 
(D 状态 预测 ， 即 


m 
Xue = DATAE 
jal 
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(5) 状态 预测 协 方差 矩阵 ， 即 
Pik- = >u, PE = Širi +Q, 
ja 


步骤 3: 量 测 更 新 
(OD 和 矩阵 分 解 ， 即 

Pa x Sa iSi 
(2) 计算 求 积 分 点 ， 即 

Ç jak = Sia X; 十 $w- 
(3) 求 积 点 传播 ， 即 

E; kk- =h(¢ jk-1) 
(4) 观测 的 预测 ， 即 

LP z $ whl ji) 

jal 

(5) 预测 误差 协 方差 ， 即 

Po = >u, (E; a 一 £a XE; a - £i ) *R, 
(6) 计算 互 协 方差 矩阵 ， 即 

Pa = > u; (€, 7 Ñuka (£; ua E £i y 

jal 

CD 计算 卡尔 曼 增益 ， 即 

K, = P. P-' 
(8) 状态 更 新 ， 即 

X, = Xqa + K, (z, — Z3) 
(9) 状态 协 方差 矩阵 更 新 ， 即 

P, = Pik - K, P, K; 


5.2.3 “平方根 实现 算法 


在 工程 应 用 中 ，CKF 算法 在 递 推 计算 时 ， 由 于 计算 机 字 长 有 限 而 产生 的 舍 入 误差 可 
能 导致 误差 协 方 差 矩阵 失去 对 称 性 和 正定 性 ， 从 而 影响 滤波 器 的 数值 稳定 性 。SR-CKF 
通过 引入 正 交 三 角 分 解 回避 了 直接 对 和 矩阵 开 方 运算 ， 可 提高 滤波 器 的 稳定 性 。 

SR-CKF 中 方差 阵 可 写 为 P=44T7 ， 考 虑 正 交 三 角 分 解 4 =QR ，Q AEX, RX 
上 三 角 阵 ， 于 是 有 

P=447=RIOIOR=RTIR=SST 

引入 下 列 三 组 记号 : 

(1) S-Tria(4) 表示 一 种 三 角 化 运算 。 

(2) So(Kk) 和 Sk 分别 表示 过 程 噪声 和 测量 噪声 方差 的 平方 根 。 

假设 在 k 时 刻 已 经 得 到 kk 一 1 时 刻 的 滤波 估计 盛 | 及 其 误差 协 方差 阵 的 平方 根 矩 阵 


83 


Se， 则 大 时 刻 的 状态 估计 值 总 及 其 方差 阵 的 平方 根 S 可 以 利用 SR-CKF 递 推 计算 求 
得 ， 具 体 步骤 可 概括 如 下 。 

Alg (5.3): Square-Root Cubature Kalman Filtering 

1.， 时间 更 新 

(OD 计算 求 容积 点 〈 j=1,2…,m )， 即 


ea =S, A X; t Xa 
AP: m-2n, Cn, 为 状态 量 的 维 数 ); S38 x, (6 为 n, 阶 单位 向 量 ) 由 下 式 给 出 
Vmi2 -£,, j=1,2,…n, 

vs -E Tas JUM tla, +2,.…,2n, 

(2) 计算 传播 后 的 求 容 积 点 ， 即 
jak- 7 SÈ) 

(3) 状态 预测 估计 值 ， 即 
Xie-1 = D 


(4) 预测 误差 方差 阵 的 平方 根 ， 即 
Spr 7 Trialera So) 


式 中 
ei lega — Xi Čz kk- m EE mtl - X4 a41W 
W = diag[ u), Ju, ,---, wn] 
2. 测量 更 新 
(1) 计算 求 容积 点 ， 即 


é jkk-1 = 9k ` Xj taa 
(2) 计算 传播 后 的 求 容积 点 ， 即 

Ekk- a h(Š ua) 
(3) 测量 预测 ， 即 

daa 2 u AG, ai) 

j=l 

(4) 计算 方差 阵 的 平方 根 ， 即 

S kai = Tria([8,, Sx]) 


Ei =[E a CA ,Ek EE [n yt SE kl 一 Za] 


(5) 计算 互 协 方差 矩阵 ， 即 
Pa = 2v, (€ ki 7 Šik- X£, ui - Ln r =Z, Eh 
j= 


(6) 计算 卡尔 曼 增益 ， 即 
K, = (PISZ YS} 
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(7) 状态 更 新 ， 即 


%, = Xia + Ki (zh — £43) 


(8) 状态 协 方差 矩阵 更 新 ， 即 
S, = Tria([,, , 一 kai K,S,]) 
5.3 应 用 实例 


5.3.1 常规 点 目标 跟踪 


考虑 二 维 平面 中 的 目标 跟踪 问题 , 假定 上 时 刻 目 标的 状态 为 xx =[x X y y (0 ,系统 
状态 的 演化 过 程 满足 


x, =Fx,_ +W, (5-16) 
式 中 
sls °], ml | iim 
0 F 0 1 
7 为 采样 间隔 ， wi 为 零 均 值 高 斯 过 程 噪声 ， 其 协 方差 矩阵 为 
3 2 
oe [2 1] a [22 77 cam 


传感器 位 于 坐标 原点 ， 观 测 方程 为 
nhe) Visi uM (5-19) 
arctan(y,/x,) | (Ye 

AP: vU EIS B PS rB H. R, — cov[v, ]-diag([0? os]) 。 

仿真 中 ， 假 定 目标 初始 状态 为 x,-[40km -10043m/s 60km -100n/s]' ， 初 始 估计 
误差 协 方差 矩阵 为 P,=diag([3002,1002,3002,.1002]) , g-2m/s!, T=ls, o,-100m, 
0o,=03. RH 3 点 高 斯 - 厄 米 特 求 积 的 QKF、CKEF 及 EKF 算法 分 别 对 目标 进行 跟踪 ， 
实验 次 数 为 500， 所 得 结果 如 图 $.1 一 图 5.5 所 示 。 作 为 比较 ， 图 中 同时 给 出 了 目标 跟 
踪 的 误差 下 限 PCRB。 


axis-y(m) 





28 30 32 34 36 38 40 42 


axis-x(m) x104 


图 5.1 单 次 跟踪 示例 
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RMSE of position estimation for axis-x(m) 
RMSE of position estimation for axis-y(m) 


0 10 20 30 40 50 60 0 10 20 30 40 50 60 
Time(s) Time(s) 
图 52 x 轴 位 置 估 计 均 方 根 误差 图 5.3 》 轴 位 置 估计 均 方 根 误差 
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RMSE of velocity estimation for axis-x(m/s) 
° 


RMSE of velocity estimation for axis-y(m/s) 


0 10 20 30 40 50 60 0 10 20 30 40 50 60 
Time(s) Time(s) 
图 54 x 轴 速度 估计 均 方 根 误差 图 5.5 了 轴 速度 估计 均 方 根 误差 


由 图 $.2 一 图 5.5 中 的 结果 可 以 看 出 ， 三 种 算法 的 估计 性 能 非常 接近 ， 尤 其 是 在 跟踪 
收敛 之 后 ， 几 种 算法 的 误差 曲线 几乎 重合 。 综合 本 实例 及 3.4.1 中 的 结果 可 以 看 出 , TERR 
f (这 里 主要 是 观测 方程 ) 非 线性 程度 不 高 、 且 初始 误差 适中 的 条 件 下 ，EKF 具有 与 
CDKF. QKF 及 CKF 类 似 的 性 能 ， 且 从 图 5.1— E 5.5 的 结果 看 ， 这 一 性 能 已 接近 PCRB 
误差 曲线 ， 说 明达 到 了 近似 最 优 的 估计 。 

表 5.1 给 出 了 三 种 算法 的 执行 效率 比较 ， 由 此 可 以 看 出 ， 单 次 仿真 中 CKF 的 平均 时 
间 开 销 约 为 EKF 的 3 倍 ， 而 QKF 的 时 间 消 耗 比 CKF 稍 高 ， 这 与 QKF 算法 中 利用 了 更 
多 的 求 积 点 计算 的 事实 是 一 致 的 。 

表 5.1 三 种 算法 运行 时 间 比 较 


Mean/ (s/run) 
STD Gu) mea 





532 再 入 目标 跟踪 
进一步 考虑 再 入 目标 跟踪 问题 , 假定 时 刻 目标 的 状态 为 x =[x x y y] (k) ,系统 状 
态 的 演化 过 程 满足 
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x, =f (x, ,)*w, -Fx, ,* Gf, (x, ,)* Gf, +w, (5-20) 


[F 0 Jar 5 s _|T°/2 à 
Am: a M aP | G ^ xi G, | P | wi 为 零 均 值 高 斯 过 程 品 


n [Q 9 Tm Tu 
声 ， 且 其 协 方差 矩阵 为 Q,=4 k ali 2p 2 T 为 采样 间隔 ; 
fia -0.5Z_o(y, 32.992 y 1a T 为 气动 阻力 对 再 入 目标 所 产生 的 阻力 加 速 


B 
FE; f,-[0—- gl 为 地 球 引力 产生 的 加 速度 P 为 空气 密度 ， 当 高 度 小 于 某 一 值 后 ， 可 建 模 
为 随 目标 高 度 下 降 按 指数 衰减 的 函数 ， 即 p(y)ece ° (c. c, 为 常数 )。 
假定 传感器 位 于 坐标 原点 ， 观 测 方程 为 


2 2 y 
yh) =| V Ua | A (5-21) 
arctan(y,/x,) | | vox 


AP: vi 为 零 均值 高 斯 噪声 且 R=cov[v, ]-diag([o? o; ]) 。 
假定 目标 初始 状态 为 x =[200km -1.5V3km/s 60km -1.5km/s]" ,目标 状态 的 初始 估 
计 误 差 协 方差 矩阵 为 已 =diag([1000?,100?,1000?,1002]) ， 其 他 参数 设置 见 表 5.2. 
X52 仿真 参数 设置 
La |z |< s | a | a | s | ae | 
[ ame | | sm | ox | inm | sese | ose | oo ms) — _ 


由 于 本 实例 中 的 系统 方程 具有 较 强 的 非 线性 , 若 采 用 EKF 算法 求 Jacobian 矩阵 将 非 
常 繁杂 ， 因 此 这 里 主要 考虑 采用 免 导数 计算 的 UKF、QKF K CKF 算法 分 别 对 目标 进行 
跟踪 ， 仿真 次 数 为 500， 所 得 结果 如 图 $.6 一 图 5.10 所 示 。 





0.8 1.0 1.2 1.4 1.6 1.8 2.0 22 
axis-x(m) x10* 


图 5.6 单 次 跟踪 示例 
由 图 5.6 一 图 5.10 中 的 结果 看 , 与 前 一 实例 中 完全 一 致 的 是 ,， QKF 及 CKF 具有 完全 


相似 的 性 能 ， 在 图 中 表现 为 两 种 算法 的 误差 曲线 几乎 完全 重合 ， 相 比 之 下 ，UKEF 算法 在 
跟踪 达到 稳定 前 的 性 能 略微 占 点 优势 。 表 53 给 出 了 几 种 算法 的 时 间 复 杂 度 比较 ， 由 此 
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可 见 CKF 与 QKF 时 间 开 销 较为 接近 , 但 比 QKF 稍 低 ， 这 与 前 面 的 仿真 结果 一 致 。 从 表 
中 的 结果 同样 可 以 看 出 ，UKF 算法 的 时 间 开销 稍 大 ， 这 主要 是 源 于 两 方面 的 原因 : 一 是 
算法 实现 时 采用 的 是 UKF 的 通用 算法 , 未 考虑 加 性 噪声 条 件 下 的 简化 , 而 后 面 两 种 算法 
直接 利用 了 加 性 高 斯 噪声 的 特点 ， 二 是 在 QKF 和 CKF 算法 中 计算 预测 样 点 时 采用 了 和 所 
阵 乘积 运算 ，UKF 中 采用 的 则 是 逐 点 计算 的 方法 。 综 合 比较 得 知 ，CKF 在 处 理 此 类 问题 
中 具有 一 定 的 折 中 优势 。 


5S š š 
s 23 


Š 
Š 


š 
š 


š 
E 





RMSE of position estimation for axis-y(m) 
I] 
s 


RMSE of position estimation for axis-x(m) 
tw 
S 


e 


30 40 50 60 0 10 20 30 40 50 60 
Time(s) Time(s) 


图 5.7 xZX 6 X4 ZARXÉ 图 5.8 ?方向 位 置 估计 均 方 根 误差 


° 
s 
N 
e 





RMSE of velocity estimation for axis-x(m/s) 
RMSE of velocity estimation for axis-y(m/s) 


0 
10 20 30 40 50 60 
Time(s) 


图 5.9 方向 速度 估计 均 方 根 误差 图 5.10 》 方 向 速度 估计 均 方 根 误差 
表 5.3 三 种 算法 运行 时 间 比 较 
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54 本 章 小 结 


对 于 一 般 的 非 线性 系统 ， 在 卡尔 曼 滤波 框架 下 ， 状 态 的 预测 分 布 和 后 验 分 布 都 被 近 
似 为 高 斯 分 布 ， 同 时 ， 由 经 典 贝 叶 斯 滤波 的 两 个 步骤 可 知 ， 状 态 预 测 和 更 新 算式 可 表示 
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成 状态 分 布 与 非 线性 系统 函数 乘积 的 多 维 积分 形式 ， 这 种 积分 形式 的 近似 解 可 通过 高 斯 
一 厄 米 特 求 积 公式 得 到 (当然 也 可 通过 其 他 求 积 分 规则 得 到 , 但 高 斯 - 厄 米 特 积分 较为 常 
用 )。 相 应 地 ， 基 于 该 计算 原理 的 卡尔 曼 滤波 算法 称 为 求 积 卡尔 曼 滤波 〈(QKF) 算法 。 对 
Tm 点 高 斯 - 厄 米 特 求 积 公式 , 其 近似 精度 为 2m -1 阶 多 项 式 ， 显 然 这 种 方法 对 于 单 变量 
非 线性 问题 的 求解 来 说 具有 精度 和 效率 方面 的 优势 。 理 论 上 ， 特 定 的 权 值 条 件 下 ， 三 
点 高 斯 一 厄 米 特 求 积 结果 与 不 带 尺 度 因 子 4 的 UT 得 到 的 结果 是 等 效 的 。 从 这 一 意义 来 
说 ， 由 此 得 到 的 QKF 和 UKF 也 应 具有 相同 的 性 能 ， 这 一 点 已 被 前 文中 的 实例 所 证 实 
(图 5.7 一 图 5.8 中 的 两 种 方法 在 性 能 方面 的 细微 差异 是 由 于 UKF 算法 中 的 参数 设置 引 
起 的 )。 

对 于 多 维 状态 量 , 在 相同 的 精度 要 求 下 ,高 斯 - 厄 米 特 求 积 计算 所 需 的 求 积 点 个 数 随 
状态 量 的 维 数 呈 几何 级 数 增长 ， 这 对 于 高 维 状态 问题 的 求解 是 极为 不 利 的 。 最 新 的 研究 
表明 ， 利 用 一 种 特定 的 求 容积 规则 ， 可 以 在 保证 近似 精度 损失 不 大 的 前 提 下 有 效 减 小 求 
积 点 数 ， 从 而 简化 运算 量 。 通 过 典型 的 目标 跟踪 实例 证 实 ， 基 于 求 容积 规则 的 求 容积 卡 
尔 曼 滤波 (CKF) 算法 在 性 能 方面 与 QKF 几乎 完全 相当 ， 具 有 较 好 的 扩展 应 用 前 景 。 
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第 6 章 混合 高 斯 近似 滤波 


第 3 章 一 第 5 章 主要 从 非 线性 系统 不 同 的 近似 手段 这 一 角度 ， 阑 述 了 次 优 非 线性 滤 
波 估计 问题 。 但 是 应 该 注意 到 ， 讨 论 这 些 方法 时 隐 含 了 一 个 重要 的 假设 条 件 ， 即 系统 状 
态 是 单 模 态 的 ， 也 就 是 说 ， 无 论 是 预测 PDF 还 是 后 验 PDF 都 仅 用 一 个 高 斯 分 量 来 近似 。 
理论 和 实践 都 证 明 这 种 近似 方法 的 有 效 性 仅 对 单 模 态 问题 成 立 (假设 系统 状态 和 噪声 都 
是 高 斯 分 布 ， 一 个 高 斯 分 量 对 应 一 个 模 态 )。 

然而 现实 世界 中 面临 的 很 多 问题 往往 比 单 模 态 问题 复杂 得 多 ， 一 个 典型 的 例子 是 系 
统 噪声 〈 过 程 /观测 噪声 ) 不 是 理想 的 高 斯 分 布 ( 如 Gamma 分 布 或 其 他 更 复杂 的 混合 分 
布 )， 此 时 ， 建 立 在 高 斯 噪声 假设 基础 上 的 各 种 高 斯 近似 滤波 算法 由 于 模型 失 配 等 因素 的 
影响 难以 获得 理想 的 性 能 。 另 一 个 典型 例子 是 系统 状态 本 身 具 有 多 模 性 ， 如 在 目标 跟踪 
领域 ， 单 模 态 的 状态 量 很 难 对 多 个 目标 进行 充分 、 有 效 描述 。 对 于 诸如 此 类 的 问题 ， 前 
面 介绍 的 几 类 近似 方法 显得 无 能 为 力 。 

解决 这 一 问题 的 一 种 可 行 途 径 是 采用 多 模 近 似 方法 ， 即 采用 多 个 高 斯 分 量 组 成 混合 
高 斯 分 布 来 有 效 近似 多 模 态 问题 ， 混 合 高 斯 滤波 就 是 基于 这 一 思想 提出 来 的 。 需 要 指出 
的 是 ， 由 于 任何 一 种 复杂 的 非 线性 系统 都 可 能 用 混合 高 斯 分 量 的 形式 达到 任意 精度 的 近 
似 ， 从 这 一 点 来 看 ， 混 合 高 斯 滤波 算法 自然 也 适用 于 单 模 问题 。 

本 章 主要 介绍 基于 混合 高 斯 近似 的 滤波 方法 ， 首 先 给 出 了 非 高 斯 分 布 的 混合 高 斯 
近似 原理 和 方法 ， 在 此 基础 上 结合 EKF、UKF 和 QKF 三 种 典型 的 滤波 框架 ， 得 到 了 
相应 的 混合 高 斯 滤波 算法 ， 最 后 给 出 了 闪烁 噪声 条 件 和 可 变数 目 条 件 下 的 目标 跟踪 应 
用 实例 。 


6.1. 非 线性 系统 的 混合 高 斯 近似 


在 开始 阐述 具体 算法 前 ， 首 先 介绍 一 个 重要 引 理 。 
28b. [£3 n HA x 的 概率 密度 函数 p(x) 可 通过 下 式 


I I 
po)-Y wN Gu, Pp), Y u,=1 (6-1) 
i=l i=] 


进行 近似 ， 且 只 需 分 量 个 数 了 足够 大 ， 其 近似 误差 可 达到 任意 小 。 

关于 该 引 理 的 详细 证 明 可 参阅 文献 [30], 利用 这 一 近似 过 程 , 可 以 处 理 一 些 与 非 高 斯 
现象 相关 的 经 验 分 布 《 如 2 混合 噪声 ， 它 被 广泛 应 用 于 描述 非 高 斯 噪声 环境 ， 如 跟踪 过 
程 中 的 闪烁 噪声 、 干 扰 或 杂 波 等 )。 通 过 混合 高 斯 学 习 算法 ， 混 合 高 斯 分 布 可 以 按 任意 近 
似 精 度 逼 近 非 高 斯 噪声 密度 ， 常 见 的 学 习 算 法 有 期 望 最 大 化 (EM) 算法 和 kk- 均值 聚 类 算 
法 所 等 。 因 此 ， 对 于 如 下 式 所 示 的 系统 模型 
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E, (Xa), (6-2) 

y, = h(x,) + v, (6-3) 

若 其 噪声 为 混合 高 斯 加 性 噪声 , 则 其 预测 密度 和 后 验 密度 也 可 表示 为 高 斯 和 的 形式 。 
具体 地 ， 假 设 加 性 过 程 噪 声 和 观测 噪声 可 分 别 近似 为 


I 
p(w,) = Y a, ON [ww (2,0, G)] (6-4) 
i71 
J 
p(v,) = Y. (DN [v,;y, GR, ()] (6-5) 
j71 


式 中 
I J 
Ya04, YXA04 
i=1 j=1 
进一步 假设 初始 时 刻 的 先 验 密度 也 为 高 斯 和 分 布 ， 即 


pæ p G)- 3: ON D) (6-6) 
高 斯 和 滤波 要 求 通过 一 1 € ug la, 1) 得 到 时刻 的 时 间 更 新 先 验 分 布 

PG la a) 9 È aipa WN (3,514 aG), (7) (6-7) 
及 测量 更 新 后 验 分 布 

pula) s > (nN (x, ;&, (),P, (n)) (6-8) 


X (6-8) 计算 过 程 中 ， 由 于 状态 转移 方程 的 先 验 分 布 p@x,|x, ,) 可 表示 为 
pls, , FEAON(. f (4)9,0).0, 0) (6-9) 
于 是 有 
pala. "Ya. OBO) [,.. N (x, OPa) 
N (x if(x, it, (),Q, 0)) dx, , 
~ 3 asa (DN (x,, (iy), Pa. (iy)) 


j=1 i=] 


(6-10) 


I-J 
=} ara (N (xta a CP (7) 


AP: au (rye, a Añ ü), Xa (r) i P4, C) 则 可 通过 非 线 性 高 斯 滤波 器 (EKF、QKF 
或 UKF 等 ) 的 时 间 更 新 步骤 得 到 。 
同 理 ， 观 测 方程 的 似 然 分 布 可 近似 为 


p(z,b, yx» H Q)N (z, h(x, y*v, (p),R, (p)) (6-11) 
p? 


由 此 可 得 
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£ HI 
Prl) = D 2 (Day. () fon N (1 CP 0) 
p=1 r=] 
-N (z hi), (p),R, (p)) dx, 
L IJ 
=) 2 (p)a,(r)W (x, &,(rp),P,(rp)) 


p=1 r=] 


(6-12) 


I-J-L 


E b3 G, (nN (x, X, (n),P, (n)) 
n=] 


AP: k (MEN (y, Jii (P, (n)) ， Qk (n)72, (r): m (n): Hk (p) , Jua (n) ` P, m) 为 
第 ?个 分 量 观测 的 预测 及 预测 误差 协 方差 矩阵 。 


在 MMSE 准则 下 ， 最 终 的 状态 估计 及 协 方 差 可 通过 以 下 定理 计算 得 到 。 
设 p(x) 为 包含 n 个 高 斯 分 量 的 归 一 化 混合 高 斯 分 布 且 


pG)-Y uN Gu P), Yw 
i=l i=l 
又 设 p(x) RAE HM. EHE SEE FI 
u- Yun, 
Z-Y w[P.«(u - pu- n] 


则 p, (x) 与 p(x) 的 前 二 阶 矩 〈 均 值 和 方差 ) 相等 。 

同时 由 式 〈6-12) 可 以 看 出 ， 由 于 过 程 噪声 和 观测 噪声 的 非 高 斯 特性 ， 经 过 一 次 完 
整 递 推 后 ， 高 斯 分 量 由 了 变 为 LJ 。 显 然 ， 在 这 种 条 件 下 若 不 进行 适当 的 处 理 ， 由 于 高 斯 
分 量 的 不 断 增 长 ， 混 合 高 斯 滤波 是 很 难 实现 的 。 接 下 来 将 针对 这 一 问题 ， 介 绍 高 斯 分 量 
的 缩减 方法 。 

高 斯 分 量 缩减 方法 通常 可 分 为 三 类 : 也 决策 指导 的 方法 ， 即 高 斯 分 量 的 删 减 根据 特 
定 的 决策 规则 来 进行 ，@@ 随 机 删除 方法 ， 即 随机 选择 部 分 分 量 进行 删除 ，@@ 拟 贝 叶 斯 近 
似 方法 。 在 涉及 需要 动态 删 减 高 斯 分 量 的 问题 中 ， 拟 贝 叶 斯 方法 最 为 常用 ， 通 常 包含 以 
下 几 种 策略 。 

(1) 前 枝 。 前 枝 的 含义 即 舍弃 高 斯 和 中 那些 影响 不 显著 的 分 量 而 保留 那些 影响 显著 
的 分 量 ， 通 常 可 通过 选择 部 分 权 值 最 大 的 分 量 、 售 弃 部 分 权 值 最 小 的 分 量 或 者 采用 门限 
的 方法 进行 选择 。 

(2) 合并 。 在 这 种 策略 中 ， 用 于 两 高 斯 分 量 ; 与 /之 间 的 相似 性 可 用 Mahalonobis 距 
Ad KEE, B 
Qa. 

— (x, -x,) Z x, -x;) (6-13) 


式 中 :2 为 高 斯 和 的 组 合 方差 ，w Ma 两 分 量 的 权 值 。 
当 帮 小 于 某 设 定 门限 时， 将 两 个 分 量 进行 合并 ， 得 到 新 的 高 斯 分 量 为 





d; = 
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BUB: «.-o,;*a; 
均值 : X= 





Q,X,ta,x 
n F j j) 
i J 
1 QQ; 


[e.Z+a Z + 
a taj ata; 








方差 : E (x, - xx, - x,)' ] 


62 ”混合 高 斯 非 线性 滤波 


由 前 面 的 分 析 可 知 ， 采 用 混合 高 斯 分 布 可 以 近似 非 高 斯 噪声 。 现 在 问题 的 关键 是 ， 
如 何 得 到 式 〈6-10) PRIN Ge, xu 0) Pu a0) 及 式 (6-12) PH AN Ge, x, Q0),P, (0) 。 在 
卡尔 曼 滤 波 框架 下 ， 根 据 不 同 的 近似 手段 ， 如 一 阶 泰 勒 展开 、UT 变换 、 高 斯 - 厄 米 特 求 
积 ， 得 到 相应 条 件 下 的 高 斯 和 滤波 算法 GM-EKF、GM-UKF、GM-QKF 等 。 下 面 对 这 三 
种 最 为 典型 的 滤波 方法 进行 详细 分 析 ， 其 他 近似 手段 得 到 的 滤波 算法 与 此 类 似 。 
6.2.1 高 斯 和 扩展 卡尔 曼 滤波 

在 高 斯 和 扩展 卡尔 曼 滤 波 (GM-EKF) 中 ， 状 态 的 预测 分 布 和 滤波 分 布 均 被 近似 为 
高 斯 和 的 形式 ， 假 设 系 统 状态 、 过 程 噪 声 及 观测 噪声 的 高 斯 分 量 数 分 别 为 IT J fn L, MM 
一 次 完整 的 递 推 过 程 可 概括 如 下 。 

Alg (6.1): Gaussian Mixture Extended Kalman Filtering 

步骤 1: 初始 化 

ZEN Ew, G), X0), PG) (i=1,2…,1)， 使 


I 
pC) 2 wy ON x); SG),P,G)] 


步骤 2: 预测 
F, 4.) 7 V.f Eo un (6-14) 
Xa) = Ska w,U)  r-G-1J9 (6-15) 
P, (r) = F,G)P, (DFF G) + Q,G) (6-16) 
Uy a) w, G), 0) (6-17) 
步骤 3: 修正 
H,(r)=V,h(x)|,.,, 0 (6-18) 
Pip Gp) h£ a (THY (p) (6-19) 
Ki (rp)= P, 4C)H; (UT, (Pi s (rH; (r) + R,(p)] ' (6-20) 
(n) = fip r) + KPD — atr) n=(r-1)L+p (6-21) 
P,(n) = P4, ,(r) - K, p)H,(r)P,, (r) (6-22) 
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u, (1)- 7 - (r)- m(n): 4 (p) 


ES wyma) mO) pn 
r=] 1=1 
NDEN (yrds (rp) Hi (Pa ,()H (r) + R,(p)) (6-24) 
步骤 4， 估计 
IJL 
&, = 3 d, (n), (n) (6-25) 
=| 
IJL 
P=} ü, (n)[ P,(ny+[$, 00) — 2,1, 0 — 3, T' ) (6-26) 
n=1 
步骤 5, NH 
(à, (n),&, @),P,(n))⁄, {DDR ,POY (6-27) 


式 中 ， 人 多 (9 为 wi(n) 的 归 一 化 结果 。 


6.2.2 ”高 斯 和 不 敏 卡尔 曼 滤 波 

将 混合 高 斯 分 布 模型 与 UKF 算法 相 结 合 ， 即 可 得 到 高 斯 和 不 敏 卡 尔 曼 滤波 
(GM-UKF) 算法 如 下 。 

Alg (6.2): Gaussian Mixture Unscented Kalman Filtering 

步骤 1: 初始 化 


I 
Po 六 2 w ON [xo (i); G),P, G)] (6-28) 
i=1 
(i)=E[xo()] (6-29) 
PG) = E| (x6) - 3,6) (x96) - $O) | (6-30) 
步骤 2: 样 点 计算 
ial) = [Bl) 2,6 + CET AO (631) 
步骤 3: 时 间 更 新 
Zra Ò) = f[x, aG)] (6-32) 
2L 
Ñu a (r) = You vas (D+w (j) (6-33) 
1-0 
2L 
P,,,()-Q, 0*2. wj? gi Op- aus 
[Xi (r) - W, Ork- (r) - W, OT 
Vr- O) = PEZ aO] (6-35) 
2L 
ROED DIEA T 07,0) (6-36) 
1-0 
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步骤 4: 测量 更 新 
2L 

P, (r p)- 2. wj Hasa (r 9a (7)— TE (n) - v, (PP. (n) - v, pT +R,(p) (6-37) 
1=0 


2L 
Py (rp)- 2: wj na (r Jip- (r)- Šik- (r Duca (n)- v, (p) (6-38) 
1-0 
K,(rp) = P, (rp)P,, (rp) (6-39) 
£0) = X, G) + K,(rp)y, - uar) — n7 - DL G8402 
P,(m) = P(r) - K, (r.p)P,,(r.p)KT (rp) enia 
N ap 2e rh = = e> Sod (6-42) 
Y luu a (r) m (n): up) 
r=1 1=] 
NDEN (y, fa rp),P, (rp)) (6-43) 
步骤 5: 估计 输出 
IJP 
£, = > ` @, (n)%, (n) (6-44) 
n-l 
IJP 
P=) à, (n)( P, aR 0) - , -T ) (6-45) 
n-l 
步 又 6: 分 量 剪 枝 
{DE POE > (G7, ORORO da 


62.3 ”高 斯 和 求 积 卡尔 曼 滤 波 


同 理 ， 将 高 斯 混合 分 布 模型 与 QKF 相 结合 ， 即 可 得 到 高 斯 和 求 积 卡尔 曼 滤 波 
(GM-QKF) 算法 ， 具 体 流程 可 概括 如 下 。 
Alg (6.3): Gaussian Mixture Quadrature Kalman Filtering 
步骤 1. 时 间 更 新 
(OD ERES, BR 
P, ,(r) - S, (r Mia ), Gao G): B,0) Cr-G -1)- I) 
(2) 计算 求 积 点 ， 即 
eua = S,aG X +X, G), I=1,2,…,m 
(3) 传播 求 积 分 点 ， 即 
Či kk- =f (6,4) 


(4) 状态 预测 ， 即 
Xie (r)= XX an tm g) 
I=] 
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(5) 状态 预测 协 方差 矩阵 ， 即 
Paar) = 5 v6 了 )- [Xi (r) - w,G Wirk- (r)- wW, or *Q,() 
l=1 


步骤 2; 量 测 更 新 
(D 和 矩阵 分 解 ， 即 
Pox- )= Sui (r Siia (r) 
(2) 计算 求 积分 点 ， 即 
erga) = Sua (n): Xi + Xa) 
(3) 求 积 点 传播 ， 即 
€ ua (r)7 h[8, ca (r)] 
(4) 观测 预测 ， 即 
š (rp) = 2 uH, au COL) 
1=1 
(5) 观测 预测 协 方差 矩阵 ， 即 
P. (rp) = 2 0E y Ee) Dag l) 79, 0E (r) v (I +R, p) 
Iz 
(6) 计算 互 协 方差 矩阵 ， 即 
P xz,k|k-1 (rp)= > mia (r i (r)- Xa (r Za (r) -v, (p)r 
l=] 
CD 计算 卡尔 曼 增益 ， 即 
K,(rp) = P,,(rp)P;;' (rp) 
(8) 状态 更 新 ， 即 
X,(n) = Sipa (r) + K, (r.p)[ç, — gÚ rp) 
(9) 状态 协 方 差 矩阵 更 新 ， 即 
P,(n)= Pa (r) - K,(r PIP aa (r DK; (rp) 


上 述 处 理 流程 中 ， 权 值 更 新 算式 与 高 斯 分 量 剪 枝 方法 与 Alg (6.1) 中 的 完全 相同 ， 
在 此 不 再 重复 。 


6.3 应 用 实例 


6.3.1 ”闪烁 噪声 下 的 目标 跟踪 


仍 以 二 维 平 面 内 的 目标 跟踪 为 例 ， 假 定 系统 方程 由 式 (5-16) MA (5-19) AR, A 
中 各 参数 的 含义 与 前 文 相同 ， 所 不 同 的 是 距离 和 方位 观测 噪声 不 是 理想 的 高 斯 噪声 ， 而 
是 非 高 斯 闪烁 噪声 。 对 于 雷达 闪烁 噪声 的 建 模 ， 通 常 可 近似 为 混合 高 斯 分 布 或 高 斯 与 拉 
普 拉 斯 的 混合 分 布 ， 对 于 后 则 ， 由 于 拉 普 拉 斯 分 布 一 般 难 以 直接 处 理 ， 因 此 一 般 建 模 成 
具有 多 分 量 的 混合 高 斯 分 布 ， 并 利用 EM 算法 得 到 各 高 斯 分 量 的 参数 及 权 系 数 。 

为 考虑 问题 方便 ， 假 定 闪 烁 噪声 建 模 为 具有 两 个 不 同 分 量 的 混合 高 斯 分 布 ， 即 
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p(v,)'(1— E)p (v,)*& p,(v,) 
=(1—#)W(v,,R.)r+e N(v,:z,,R,) 


AP: £ 为 闪烁 噪声 出 现 的 概率 ， 该 值 越 大 说 明 目 标 闪 烁 越 频繁 。 
仿真 中 ， 假 定 目标 初始 状态 为 x0=[20km -160m/s 25km -120m/s]! ， 初 始 估计 误差 
协 方差 矩阵 为 己 =diag([200?,1002,2002,1002]) ， 其 余 参 数 见 表 6.1。 


表 6.1 仿真 参数 设置 


采用 EKF, GM-EKF 及 GM-UKF 算法 分 别 对 目标 进行 跟踪 ， 其 中 EKF HARHA 
匹配 方法 ， 即 
H=E[v, (1 - £) +e, 70 
R-E|(v, — u)v, 一 yu ]*(1- £)R, *eR, 
而 GM-EKF 和 GM-UKF 算法 中 ， 系 统 状态 采用 16 个 高 斯 分 量 ， 每 个 分 量 在 真 值 附 
近 采 用 如 随机 生成 ， 各 分 量 的 初始 权重 相等 ， 即 


16 
p(x,)= > w (DN (xoli) P (i) 
i=] 


过 程 噪声 采用 单 分 量 ， 其 量 值 与 真实 的 过 程 噪声 一 致 ， 而 量 测 噪声 建 模 为 2 个 高 斯 
分 量 ， 且 其 均值 和 协 方差 参数 与 真实 闪烁 噪声 相同 。 在 上 述 设置 条 件 下 ， 每 次 递 推 完 成 
后 ， 状 态 的 高 斯 分 量 个 数 由 16 变 为 16x 2=32 个 ， 为 保证 算法 的 可 实现 性 ， 采 用 取 大 裁 前 
法 将 高 斯 分 量 数目 恢复 为 16。 独 立 运行 500 次 蒙特 卡 罗 仿 真 ， 所 得 结果 如 图 6.1 一 图 6.5 
所 示 ， 为 图 示 清 楚 ， 图 6.1 给 出 的 是 局 部 放大 的 结果 。 

由 图 中 的 结果 可 以 ,在 本 仿真 参数 设置 条 件 下 ，GM-EKF 和 GM-UKF 的 性 能 非常 接 
it, 且 比 EKF 算法 要 有 明显 改善 (特别 是 对 于 目标 位 置 估计 )。 这 就 说 明 : (DEKF 和 UKF 
估计 算法 本 身 的 性 能 在 该 实例 中 是 相当 的 ，@ 在 非 高 斯 〈 闪 烁 ) 噪声 条 件 下 ， 影 响 目标 
跟踪 精度 的 主要 因素 是 模型 的 非 高 斯 性 而 不 是 方程 的 非 线 性 ;外 传统 的 基于 单 高 斯 分 量 
的 卡尔 曼 型 滤波 算法 在 处 理 非 高 斯 问题 时 将 会 导致 明显 的 性 能 下 降 。 





1.3 1.4 1.5 1.6 1.7 1.8 


图 6.1 单 次 跟踪 示例 
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图 6.2 x 轴 位 置 估计 均 方 根 误差 图 6.3 》 轴 位 置 估计 均 方 根 误差 





RMSE of velocity estimation for axis-x(m/s) 
RMSE of velocity estimation for axis-y(m/s) 


0 10 20 30 40 50 60 0 10 20 30 40 50 60 
Time(s) Time(s) 
图 6.4 工 轴 速度 估计 均 方 根 误差 图 6.5 ) 轴 速度 估计 均 方 根 误差 


为 了 比较 算法 的 复杂 度 ， 表 6.2 给 出 了 三 种 算法 运算 时 间 开 销 。 由 此 可 见 ， 采 用 混 
合 高 斯 模型 之 后 ， 算 法 的 运算 开销 也 相应 增 大 ， 且 其 增 量 略 高 于 高 斯 分 量 总 数 所 对 应 的 
倍数 。 不 过 ， 由 于 这 种 滤波 算法 具有 较 好 的 并 行 性 ， 若 采用 并 行 处 理 实现 手段 ， 在 时 间 
开销 方面 可 做 到 与 单 高 斯 分 量 相当 。 


表 6.2 三 种 算法 运行 时 间 比 较 


ve r ue | wm 
STD/ Cs/run) Y 


为 了 进一步 分 析 噪 声 闪 烁 强度 对 跟踪 性 能 的 影响 , 表 6.3 给 出 了 跟踪 稳定 建立 后 (大 
于 10s) 不 同 闪烁 概率 条 件 下 的 平均 均 方 根 误 差 (ARMSE )。 由 表 中 的 结果 可 以 清楚 看 出 ， 
随 着 闪烁 强度 的 降低 , 三 种 跟踪 算法 的 跟踪 精度 一 致 提高 ; 但 即便 是 在 s=0.05 的 情况 下 ， 
两 种 混合 高 斯 滤波 器 (GM-EKF 和 GM-UKF) 的 估计 性 能 仍 要 明显 好 于 EKF， 说 明了 混 
合 高 斯 模型 在 处 理 非 高 斯 问题 中 的 有 效 性 。 
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表 6.3 三 种 算法 平均 均 方 根 误差 ( ARMSE) 比较 


EKF E 一 0.25 75.9842 57.5557 6.8366 5.6792 
g=0.15 62.6276 47.0547 6.2338 5.0482 
E 一 0.05 45.7380 34.5908 5.0113 4.2256 
GM-EKF < 一 0.25 59.8275 44.8822 5.8776 4.8713 
E 一 0.15 43.4483 32.4868 4.7965 3.9421 
£7 0.05 35.0325 26.5295 4.2463 3.6285 
GM-UKF 2— 0.25 59.9895 45.0083 5.8893 4.8774 
£—0.15 43.3378 32.4104 4.7164 3.9269 
£7 0.05 34.9981 26.4758 4.2433 3.6212 


6.3.2 GM-PHD 多 目标 跟踪 


本 实例 主要 利用 混合 高 斯 近似 方法 结合 概率 假设 密度 (PHD) 滤波 来 实现 对 多 目标 
的 跟踪 ， 为 此 首先 对 有 限 集 统计 的 相关 理论 作 一 简要 介绍 。 

有 限 集 统计 (Finite Set Statistics, FISST) 理论 6 给 出 了 随机 有 限 集 (RFS) 的 统计 
特性 , 利用 随机 集 理 论处 理 单 / 多 传感器 多 目标 跟踪 问题 是 行 之 有 效 的 方法 。 在 FISST 中 ， 
目标 集 视 为 一 个 全 局 目标 ， 传 感 器 (组 ) 在 相同 时 刻 提 供 的 量 测 认 为 是 一 个 全 局 量 测 ， 
多 目标 的 状态 模型 和 观测 模型 可 以 表示 为 RFS 形式 。 

假设 在 上 上 时刻， 多 目标 状态 集 为 Xe={xtaxkzxka 上， 多 目标 观测 集 为 Zr={zk1， 
和 2 Zr 。 其 中 ，M 和 :分别 表示 大 时 刻 的 目标 数目 和 量 测 数目 。 状 态 集 和 观测 集 
分 别 用 状态 空间 的 RFS X, 和 观测 空间 的 RFS Z, 表示 , 因此 , 大 时 刻 的 目标 状态 可 用 RFS 
描述 为 





X,78,4 (Xka) Y B, ,(X, ,) VT, (6-47) 
AP: Xa 和 分别 为 -1 和 大 时 刻 的 目标 状态 RFS; St- 表示 大 时 刻 仍 存在 的 目标 
RFS; Bi 为 由 Xi , 衍生 (Spawn Birth) 的 目标 RFS; T, 为 上 时 刻 瞬 间 出 现 的 新 生 (New 
Birth) 目标 RFS. 
kk 时刻 的 多 目标 观测 模型 可 用 RFS 描述 为 
Z,-9,(X,) v C,(X,) (6-48) 
AH: 0,28 X, 的 观测 RFS; C, 为 由 杂 波 或 者 虚 警 引起 的 观测 RFS. 
根据 贝 叶 斯 估计 理论 ， 基 于 RFS 的 贝 叶 斯 估计 可 描述 为 
Da. (X, Zi)" fSin- 1X lX Lp, a OC, lE Du ( dX, 1) (6-49) 
g,(Z,IX py a QXLZ ui) 
[eZ Xp aZ Ju(aX,) 


AP: Pas Pas Jak 和 g, 分 别 为 多 目标 先 验 概率 、 后 验 概率 、 转 移 概率 和 似 然 函 数 ; 
人 为 某 一 空间 适当 的 参考 测度 。 


p (X, Z, (6-50) 


若 能 通过 贝 叶 斯 理论 递 推 得 到 系统 状态 的 后 验 概率 密度 ， 就 可 方便 地 获取 其 均值 、 
方差 和 峰值 等 估计 。 但 是 ， 式 (6-49) 和 式 (6-500 需要 进行 高 维 积分 运算 ， 并 且 多 目标 
联合 似 然 函数 g, 的 计算 随 着 目标 数 的 增加 而 呈 指 数 增长 ， 使 得 直接 用 贝 叶 斯 公式 来 估计 
目标 数目 和 目标 状态 变 得 难以 实现 。 为 了 简化 计算 , 文献 [33] 利 用 多 目标 后 验 分 布 的 概率 
假设 密度 (PHD) 来 近似 代替 多 目标 的 后 验 概率 密度 ， 并 以 此 来 递 推 估算 目标 数 和 目标 
状态 ， 大 大 减 小 了 计算 负担 ， 而 文献 34]~ 文 献 [36] 则 在 此 基础 上 给 出 了 PHD 滤波 的 高 
斯 混合 近似 实现 形式 。 

概率 假设 密度 D(x) 表示 随机 有 限 集 三 的 一 阶 矩 ， 定 义 为 

p ó, e) [X5,00/:006x (6-51) 


在 多 目标 跟踪 中 ， De (xo) 表示 在 xo 点 期 望 的 目标 密度 ; [De (x)dx 表示 出 现在 监视 
区 域内 的 期 望 目标 数 ; 而 D. (x) 的 极 值 ， 给 出 了 目标 的 状态 估计 。 
PHD 滤波 通过 PHD 的 预测 和 更 新 方程 来 实现 递 推 传播 ， 其 预测 PHD 可 表示 为 
D, Xx Zir- )7Y, (x, )t [Dus (x, x, DE x, Z4.) dx, , (6-52) 


Xm: Diki (x, lx, 7b, (x,lx,., )*s,y i f x, X3); y,Q) 为 新 生 (New) 目标 的 PHD; 
ba 为 由 存在 目标 衍生 (Spawn) 的 目标 PHD; 54 为 存活 (Survival) 概率 ; Sr) 
为 转移 概率 密度 。 

当 丰 时 刻 的 量 测 z, 可 用 时 ， 其 更 新 PHD 为 


P,D 
D, (x,1Z,, )*(1- Pj )D;k-1 (x,|Zu,-i)+ > D. (x,lz,) (6-53) 
eZ, ^ Ck Dk 


AP: DEF fer (zber Dyp- (x, |Z de, ; D, (x,kz,)=g, (bx, )D,, (x, Zi- V/D) ; 
gy bx, ) 为 传感器 似 然 函 数 ， 4 为 每 次 扫描 的 平均 虚 警 个 数 (通常 建 模 为 服从 泊 松 分 布 ); 
ci(z) 为 每 个 虚 警 的 分 布 函数 ，Pp 为 检测 概率 。 
用 混合 高 斯 分 量 来 近似 多 目标 状态 的 分 布 ,并 结合 PHD 滤波 方法 即 可 得 到 GM-PHD 
多 目标 跟踪 算法 ， 下 面 以 典型 非 线 性 系统 方程 
x, = f(x, tw, (6-54) 
z, = h(x,)+ v, (6-55) 
的 形式 来 介绍 GM-PHD 算法 的 具体 实现 过 程 。 
假设 在 二 0 时 刻 ， 疡 初始 化 为 具有 .天 个 分 量 的 混合 高 斯 分 布 ， 即 


J 
D, Gl 73 UY can UU (6-56) 
则 在 预测 阶段 ， 新 生 目 标 密度 和 衍生 目标 可 分 别 用 高 斯 混合 形式 表示 为 
Jyk 
n OFS wp liN Cenalil, Zli)) (6-57) 


100 


Jk 
b, a Co)=》， wy UV Gus Li],ZorU]) (6-58) 
j^ 


AP: JI Ju 分别 为 目标 新 生 和 目标 衍生 密度 的 高 斯 分 量 总 数 ， 由 于 高 斯 混合 形式 是 
一 个 强度 函数 而 不 是 一 个 概率 分 布 ， 所 以 其 权 值 之 和 表示 目标 数目 。 
由 此 可 得 预测 PHD 函数 为 
Di (XD, ptD;, p(X) (x) (6-59) 
RP: Dua (x) Dua) 分 别 表示 存在 目标 和 衍生 目标 的 PHD。 


D, a a (x)F,, > wy UN Gc, uu IZ, uu UD) 


"o e (6-60) 
I F W, kik-1 LIN (B, ui i [i], Z, zw- UD) 
Jpk Jg- 
Da G7 Y. Y u, apu, UA Get a] Eg ly) 
n (6-61) 


Jp kik-1 
5 Y Uy, rV Gea Ir] Si lr]) 
rz] 


对 于 式 (6-54) 和 式 (6-55) 所 示 的 系统 方程 ， 式 〈6-60) 和 式 (6-61) 中 的 预测 状 
态 及 其 协 方差 矩阵 的 具体 形式 取决 于 特定 的 求解 算法 。 若 采用 基于 局 部 线性 化 的 EKF 算 
法 (采用 其 他 算法 如 CDKF. UKF 或 QKF 等 算法 流程 与 此 类 似 )， 则 


Hs TS (a, ET) 

W, a a] yw, al] 

Z all Faz, FU, 

M-A [ij], [ilta, U] 

Z,aljY-Z,4U1*-2,,U] 

F, ,[i]- 0f/üx|. 
于 是 ， 预 测 PHD 可 进一步 写成 


=k-1D0] 


Jkik-1 
Dy, 4 (x)7 Y Vg a LIN Gu [2 s) 
i=] 
AP 


J k|k-1 =J, b,k|k-1 +J yk uA 
在 更 新 阶段 ， 其 PHD 可 记 为 
D, (x)-(1 - P5)D,4 ,()* 2 DL (Zw) 
ZeZ, 

Jkjk-1 

D, ,(Zix)- y w GWN (xp U].Z, UT) 

j=l 
式 中 
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— Pallet —— 
Jkik-1 
Ac, (z)* x. Wep- lllg Clk aU) 
[=n 
gu UDEN (hun iU1),S,U]) 
BUM UK, Ullz 一 ht, UD] 
>, U]-d £ K k UH, UDZ,. -l U] 
K,U1-Z, UY(H,UT) (SW) 
S UFR, +H, UZ UY(H,UI) 
H,[]- Oh/Ox| ,0 
由 此 可 知 ， 经 PHD 更 新 后 ，k 时 刻 总 的 高 斯 分 量 个 数 为 人 =(1+|Zi1Yiw-! (Z| 表示 和 集 
AZ 的 势 ， 即 元 素 的 个 数 )。 这 样 ， 后 验 概率 密度 的 高 斯 项 随时 间 变 化 将 会 无 限制 地 增加 ， 
为 了 解决 这 一 问题 , 需要 采取 前 文 所 提 的 前 枝 、 合 并 等 措施 使 其 控制 在 一 定 的 数目 范围 之 内 。 
经 过 前 枝 、 合 并 后 ， 将 高 斯 分 量 的 权重 进行 累加 得 到 目标 数目 的 估计 Ñ. ， 由 此 可 通过 
提取 权 值 最 大 的 前 Ñ, 个 高 斯 分 量 的 均值 作为 目标 状态 的 估计 ; 也 可 通过 将 所 有 高 斯 分 量 的 
均值 进行 聚 类 的 方法 (如 k-mean 聚 类 等 ) 得 到 Ñ, 个 聚 类 中 心 ， 从 而 完成 目标 状态 的 提取 。 
下 面 仍 将 以 二 维 平面 内 的 跟踪 为 例 , 验 证 GM-PHD 算 法 在 多 目标 条 件 下 的 跟踪 能 力 。 


目标 跟踪 的 系统 模型 如 式 (5-16) 和 式 〈5-19) 所 示 ， 假 设 传感器 位 于 坐标 原点 ， 采 样 周期 
T-Is, ， 检 测 概率 P=0.99 ， 二 维 监 视 区 域 为 [200 2000]mx[0 2] ， 距 离 和 方位 观测 精度 


分 别 为 o,=2m ，ao=0.2 。 初 始 时 刻 共 有 两 个 目标 , 其 真实 状态 为 xj)= [250 2 250 -10] , 
x2)=[—25010-2500] ， 过 程 噪声 强度 为 4=0.2m/s: 。 在 k=21 时 刻 ， 由 目标 1 所 处 的 位 
置 衍生 一 新 目标 ， 其 初始 速度 为 (-10,2)m/s 3 个 目标 运动 持续 到 k=100 时 刻 ， 整 个 过 
程 中 无 新 生 目标 。 目 标 航 迹 如 图 6.6 所 示 。 


1000 


tu, U] 


false alarms 


500 


axis-y(m) 
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图 6.6 多 目标 航 迹 及 虚 敬 
假设 目标 的 存活 概率 号 =0.99 ， 每 次 扫描 的 平均 虚 警 个 数 和 =10， 新 生 目 标的 强度 为 
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y(x)-0. LA Gcpu(1],2)0. LA/ (x:u[2],2) 
式 中 
All=[250 0 2500] ; u[2]=[-2500-2500]"; Z=diag([100 25100 25]) 
衍生 目标 的 强度 为 
b(x)-0.05N (x;£,Q, ) 

式 中 : Q,-diag([100 400100400) ; “为 目标 衍生 时 的 状态 。 

仿真 中 ,假定 6=[0000] ， 每 一 时 刻 最 大 高 斯 分 量 个 数 为 J =20 。 采 用 GM-EKF 
算法 对 目标 进行 跟踪 ， 高 斯 分 量 的 前 枝 规则 同 式 (6-13) ， 其 中 门限 4d;=4， 所 得 结果 如 
图 6.7~ 图 6.8 所 示 。 
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图 6.7 ”目标 位 置 估计 结果 图 6.8 目标 数目 估计 结果 


由 图 6.7 和 图 6.8 中 的 结果 可 以 看 出 ， 在 多 目标 且 目 标 个 数 可 变 的 情况 下 ， 采 用 
GM-PHD 算法 可 以 得 到 目标 个 数 的 正确 估计 ， 并 实现 对 目标 状态 的 准确 提取 。 同 时 ， 从 
整个 算法 的 处 理 流程 可 以 看 出 ， 在 处 理 过 程 中 并 没有 用 到 传统 多 目标 跟踪 中 的 数据 关联 
算法 。 这 就 说 明 ，GM-PHD 算法 在 处 理 多 目标 跟踪 问题 时 不 仅 是 有 效 的 ， 而 且 具 有 免 数 
据 关 联 的 优势 。 
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作为 单 高 斯 分 量 近 似 的 扩展 ， 混 合 高 斯 近似 方法 为 非 线性 、 非 高 斯 条 件 下 的 滤波 问 
题 提供 了 一 条 有 效 的 途径 。 在 现实 世界 中 ， 系 统 的 非 高 斯 性 可 能 体现 在 过 程 噪声 、 观 测 
噪声 或 系统 状态 等 单一 因素 或 几 种 因素 的 综合 ， 在 这 种 情况 下 ， 单 高 斯 分 量 已 不 能 对 其 
进行 有 效 近 似 ， 这 一 点 在 闪烁 噪声 条 件 下 的 目标 跟踪 结果 中 已 得 到 充分 体现 。 与 此 不 同 
的 是 ， 混 合 高 斯 分 布 具有 以 任意 精度 逼近 非 高 斯 分 布 的 能 力 ， 由 此 得 到 的 混合 高 斯 滤波 
算法 (如 GM-EKF、GM-UKEF 等 ) 自然 能 够 适应 非 高 斯 条 件 下 的 滤波 问题 。 在 多 目标 条 
件 下 ， 目 标的 存在 、 消 失 、 衍 生 / 新 生 等 变化 过 程 同样 可 建 模 为 高 斯 混合 分 布 ， 以 此 为 基 
础 的 高 斯 混合 形式 成 为 概率 假设 密度 (PHD) 滤波 的 一 条 重要 手段 ， 在 时 变 多 目标 跟踪 
应 用 中 发 挥 着 重要 作用 。 
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与 第 二 部 分 阐述 的 基于 解析 高 斯 近似 的 方法 不 同 ， 另 一 类 递 推 型 非 线性 滤波 算法 通 
过 追踪 系统 状态 的 条 件 概率 分 布 来 实现 状态 的 估计 ， 并 通过 贝 叶 斯 定理 实现 条 件 概率 的 
转移 四。 这 类 算法 的 近似 对 象 不 是 状态 函数 或 观测 函数 ， 而 是 状态 的 条 件 概率 密度 ， 其 
目的 是 近似 真实 的 状态 分 布 而 不 是 非 线 性 函数 本 身 ， 且 用 于 近似 的 采样 样本 是 随机 的 而 
不 是 确定 性 的 。 由 于 这 类 算法 建立 在 蒙特 卡 罗 仿 真 技 术 基 础 之 上 ， 通 常 称 为 序 贯 蒙特 卡 
Y? (Sequential Monte Carlo, SMC) 法 PR 或 “粒子 ”滤波 法 (Particle Filtering, PF) FÉ, 
在 这 类 方法 中 ， 通 过 蒙特 卡 罗 仿 真 手 段 产 生 大 量 的 粒子 ， 并 由 其 散布 情况 来 逼近 状态 的 
概率 分 布 。 

由 于 粒子 滤波 通过 非 参数 化 的 蒙特 卡 罗 模 拟 方法 来 实现 递 推 贝 叶 斯 估计 ， 因 此 适用 
于 任何 能 用 状态 空间 模型 表示 的 非 线 性 系统 ， 以 及 传统 解析 高 斯 滤波 近似 算法 难以 表示 
的 非 高 斯 系统 ， 且 精度 可 以 允 进 最 优 估 计 外 。 从 这 一 点 意义 来 说 ， 解 析 高 斯 近似 方法 能 
适应 的 问题 ， 粒 子 滤波 的 方法 自然 也 能 适应 。 此 外 ， 传 统 的 解析 近似 高 斯 滤波 方法 (如 
EKF) 对 初 值 选择 较为 敏感 ， 若 初 值 选择 不 当 可 能 会 降低 收敛 速度 或 者 导致 滤波 器 发 散 。 
相 比 之 下 ， 粒 子 滤 波 器 却 有 其 独特 的 优势 ， 由 于 粒子 的 散布 性 ， 很 容易 在 一 定 的 误差 范 
围 内 快速 捕获 到 真实 的 状态 ， 从 而 提高 跟踪 系统 的 稳定 性 和 收敛 速度 。 

但 是 ， 一般 情况 下 ， 用 蒙特 卡 罗 仿 真 方法 实现 递 推 滤波 其 运算 量 要 远大 于 解析 高 斯 
近似 方法 ， 这 在 某 种 程度 上 影响 了 其 实时 应 用 。 不 过 ， 粒 子 滤波 实现 方便 且 适 合并 行 处 
理 ， 随 着 数字 处 理 技术 的 快速 发 展 该 方法 具有 广阔 的 应 用 前 景 。 

本 部 分 主要 阐述 粒子 滤波 方法 及 其 在 目标 跟踪 中 的 应 用 问题 。 第 7 章 首先 从 粒子 滤 
波 的 基本 理论 入 手 ， 闻 述 标 准 粒子 滤波 算法 的 实现 方法 和 存在 的 主要 问题 。 第 8 章 在 此 
基础 上 研究 了 基于 重要 采样 函数 的 粒子 滤波 算法 。 针 对 非 线 性 系统 中 存在 线性 子 系统 这 
一 类 重要 混合 系统 ， 出 于 降低 算法 复杂 度 和 提高 滤波 精度 等 目的 ， 第 9 章 专 门 介绍 状态 
分 布 的 “边缘 化 ”思想 及 由 此 得 到 的 边缘 化 粒子 滤波 算法 。 第 10 章 主 要 从 提高 粒子 产生 
质量 的 角度 出 发 ， 总 结 和 分 析 了 几 种 基于 智能 优化 思想 的 粒子 采样 原理 和 粒子 滤波 实现 
方法 。 


第 7 章 ， 先 验 分 布 采样 滤波 


在 非 线 性 条 件 下 , 贝 叶 斯 滤波 面临 一 个 重要 问题 是 状态 分 布 的 表达 和 积分 式 的 求解 ， 
由 前 面 章节 中 的 分 析 可 知 , 对 于 一 般 的 非 线性 / 非 高 斯 系统 , 解析 求解 的 途径 是 行 不 通 的 。 
在 数值 近似 方法 中 ， 蒙 特 卡 罗 仿真 是 一 种 最 为 通用 、 有 效 的 手段 ， 粒 子 滤波 就 是 建立 在 
蒙特 卡 罗 仿真 基础 之 上 的 ， 它 通过 利用 一 组 带 权 值 的 系统 状态 采样 来 近似 状态 的 统计 分 
布 。 由 于 蒙特 卡 罗 仿 真 方法 具有 广泛 的 适用 性 ， 由 此 得 到 的 粒子 滤波 算法 也 能 适用 于 一 
般 的 非 线 性 / 非 高 斯 系统 。 但 是 , 这 种 滤波 方法 也 面临 几 个 重要 问题 如 有 效 采 样 (粒子 ) 
如 何 产 生 、 粒 子 如 何 传递 以 及 系统 状态 的 序 贯 估计 如 何 得 到 等 。 

本 章 首 先 从 贝 叶 斯 滤波 的 随机 采样 近似 一 般 原理 出 发 ， 介 绍 了 重要 采样 技术 及 其 序 
贯 处 理 方法 ， 在 此 基础 上 针对 一 种 应 用 最 广 、 处 理 最 方便 的 特殊 最 优 采样 函数 ， 得 到 了 
标准 粒子 滤波 (SPF) 算法 的 处 理 流 程 ， 给 出 了 应 用 实例 。 


7.4 贝 叶 斯 滤波 的 随机 采样 近似 


假定 存在 如 下 非 线性 系统 模型 
Xin = f (x, Ww) (7-1) 
y, = h(x,,v,) (7-2) 
RP: x 为 系统 状态 变量 ， yy 为 观测 ，w 和 vw 分别 表示 过 程 噪声 和 观测 噪声 。 
贝 叶 斯 方法 求解 状态 估计 问题 是 通过 用 已 获得 的 观测 量 来 建立 当前 状态 x, 的 后 验 
PDF 来 实现 的 , 一 旦 获得 p(x, | mx)， 也 就 得 到 了 关于 状态 x, 某 一 映射 函数 eO) 的 估计 ; 
I(g(x,))= E, o [g(x,)] = [g(x,)p(x, | y, )dx, (7-3) 
一 般 情 况 下 ， 对 上 式 进行 解析 求解 是 比较 困难 的 ， 只 能 得 到 某 种 近似 解 。 根 据 蒙特 
卡 罗 仿 真 原理 ， 该 积分 算式 可 近似 为 


^ 1 i 
Ë, olg] 737, 8G) (7-4) 


AP: (xÜ) i=1,2,..., N) 表示 从 后 验 分 布 函数 p(x, | y.) 采样 得 到 的 N 个 独立 同 分 布 样 
本 。 当 六 充分 大 时 ， 根 据 大 数 定理 ， 其 期 望 的 估计 “几乎 总 是 ”(almost surely) 收敛 于 
真实 的 期 望 ， 即 


^ 


人 一 oo 


E l 1) [8(x, )]%rs S Epen [g (x, )] (7-5) 
同时 ， 若 g(x ) 的 后 验方 差 有 界 ， 即 
var[g(x,)] = E[g(x,): g(x,)' ]<o (7-6) 


则 由 中 心 极限 定理 外 可 得 
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和 一 oo 
JN (Elg(x,)]- Elg(x,))) — N (0,var[g(x,)D (7-7) 


Rh: — 表示 “ 依 分 布 收敛 于 ”。 
需要 指出 的 是 ， 从 后 验 滤波 分 布 p(x | yu) 进行 采样 近似 只 是 蒙特 卡 罗 仿真 技术 的 
一 种 特殊 形式 ， 实 际 上 更 一 般 的 情况 是 从 完整 的 后 验 分 布 p(x | y.) 中 进行 采样 。 


7.2 重要 采样 技术 


如 前 所 述 ， 对 于 一 般 的 非 线性 系统 ， 很 难 求 得 后 验 分 布 的 解析 解 ， 也 就 无 法 直接 从 
p(xox | y ) 进行 采样 ;一 种 可 替代 的 方案 是 通过 重要 采样 (Importance Sampling, IS) 


技术 从 某 一 已 知 的 、 易 于 采样 的 函数 进行 间接 采样 ， 这 种 函数 通常 称 为 重要 密度 函数 
(Importance Density Function，IDF)， 其 分 布 称 为 建议 分 布 〈Proposal distribution). #7 
考虑 IDF 的 具体 形式 ， 假 设 满足 要 求 的 函数 为 z(xo4 | nx) ， 则 由 重要 采样 原理 5 I, #4 


I[f(xs,)] = [f (xox Pes y) , (Xo | Pix )dxox 
(Xo, | Jy.) 


= [fe £9 a A | Vix )dxox (7-8) 
Z(xXo, | Vir) Pi) 


= |f (Xox) We (Xog) T(Xor | uk dro 
p(yix) 





wi (Xo ) = PCs |xox)P(xox) (7-9) 
Z (Xo | Yi.) 
为 方便 起 见 ， 通 常 将 ^ (xa) 简 记 为 w,， 式 (7-8) 进一步 可 化 为 
J[f(xe)] = zo; en yu, (Xo, rg, | Vir) rox 


m i (Xo4 Wy (Xo: ST (Xo. | Vix )dXo4 (7-10) 


[po | Xo.) PXo4.) EE dxox 
B (Xox Wr (Xox )n(Xo; | Ju )dxo, E. a Lf (Xor) Wy (Xo, )] 
ju, (Xox T(xXox | Vir )dxXo, I, [wy (xoa )] 


即 假定 从 建议 分 布 x(xo | yaa) 进行 采样 得 到 一 组 粒子 {x 外 ,i=1,2,…,N} ， 则 后 验 均 
值 的 估计 


x. fo )w, Gs f | 
A od Ded fot (7-11) 
— w, (x 
N i=1 k V^ 0: 


其 中 ， 归 一 化 权 值 GP 可 表示 为 


G) 
g” = wu) _ (7-12) 


到 Wy (x& 
73 序 贯 重要 采样 
若 选择 的 IDF 可 分 解 为 
Z(Xo, | Vix) = 7X, | Xox- Jia t Qua | Yi) (7-13) 


且 系统 模型 满足 以 下 三 点 假设 : 

(1) 系统 状态 服从 一 阶 马 尔 可 夫 过 程 。 
(2) 不 同时 刻 的 系统 观测 相互 独立 。 
(3) 初始 的 先 验 密度 为 po) ° 


即 
p(xox) = p(xo)[ ,px 1x2) (7-14) 
pO lxo) 7 DT, PO; 1x) (7-15) 
则 可 得 出 重要 权 的 递 推 计 算式 为 
u, = PC | Xo4)P(Xo4) 
Z(X, | Xoy-35 Jia t Xo. | Vix) 
- PG | Xox)PGGQ4 PC's a | X043) P944) 
(X, | Xo445 Jig to, a | Yi a) Prk- | Xo 3) PCXo 41) (7-16) 
=w, PG | Xox) p(xXox) 
T(X, | Xox-1 Yi )PO'a | Xor) P(Xor1) 
— pGQuy |x, )p(x, | x, 1) 
T (x, | Xo415 Jia) 
特别 地 ， 如 果 IDF 满足 
z(x, | Xy4,9) = ACE 1x4. 94) (7-17) 
则 由 该 函数 采样 得 到 的 粒子 xi) ~ m(x, | x, as yu) 建立 的 后 验 滤波 概率 密度 可 近似 为 
PG, 1) 7 Y, | (x, — x) (7-18) 
Bf (x,)] - 77 Ug f(x, (x, — xf) 9 Y, a f (xf?) (7-19) 


图 7.1 给 出 了 用 粒子 及 相应 权 (xw?) 表示 后 验 分 布 p(x | ya) 的 示例 ， 图 中 国 


图 的 大 小 代表 粒子 的 权 值 ， 权 值 越 大 表示 该 粒子 在 PDF 的 比重 越 大 〈 即 对 整个 PDF 的 
贡献 也 大 ); 反之 亦 然 。 

文献 [12] 和 文献 [13] 已 经 证 明 ， 权 值 的 方差 会 随时 间 递 推 而 增加 ， 因 此 ， 序 贯 重要 采 
FÉ (SIS) 粒子 滤波 算法 有 一 个 无 法 避免 的 粒子 退化 (Degeneracy) HA". EEKMA 
经 若干 次 递 推 以 后 ， 除 了 少量 几 个 粒子 以 外 ， 大 部 分 粒子 的 权 值 小 至 可 以 忽略 。 粒 子 退 
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化 程度 可 用 有 效 样 本 个 数 N.。 (Effective Sample Size) 来 度量 ， 其 定义 为 
N 


— S... (7-20) 
1+ var{0®} 


Na 


N. 越 小 ， 说 明 退 化 现象 越 严重 。 


后 验 密度 





图 7.1 用 粒子 表示 的 后 验 分 布 


通常 ， 有 两 种 方法 可 以 减轻 粒子 退化 问题 : 一 是 增加 重 采 样 环节 ， 二 是 选择 合适 的 
IDF 进行 更 有 效 的 采样 。 下 面 分 别 加 以 介绍 。 


7.3.1 粒子 重 采样 


减轻 粒子 退化 的 方法 之 一 是 加 入 重 采样 环节 。 重 采样 的 基本 思想 是 消除 权 值 较 小 的 
粒子 而 对 权 值 显著 的 粒子 进行 多 份 复制 ， 其 方法 是 对 后 验 密度 的 离散 近似 


P(x, | ya) = 3, af oou, — xP) (7-21) 
再 进行 一 次 采样 ， 重 新 生成 一 组 新 的 粒子 {x 四,j 21,2, N) 并 使 其 满足 
Pr(x? = 3f) 2 o (7-22) 


这 一 过 程 可 以 通过 图 7.2 加 以 解释 ， 同 样 图 中 圆圈 的 大 小 代表 所 占 的 权重 。 


1 
们 
i=l 
N 
[0] 从 
bts " za 
il 


Em 

LU] 

bte 
il 












图 7.2 重 采样 过 程 示意 


在 大 -1 时刻 N 个 粒子 具有 相等 的 权 值 ， 通 过 序 贯 采样 计算 ， 大 时 刻 粒子 的 权 值 发 生 
了 变化 。 通 过 重 采 样 环 节 , 权 值 大 的 粒子 得 到 多 次 复制 (选择 ), 权 值 小 的 粒子 被 忽略 ( 淘 
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汰 )。 由 于 新 的 粒子 独立 同 分 布 ， 重 采样 后 粒子 的 权 值 均 为 wr -MN 。 重 采样 过 程 避 免 
了 将 大 量 的 计算 耗费 在 那些 只 占有 微小 量 权 的 粒子 上 对 PDF 的 贡献 几乎 可 以 忽略 )， 
降低 了 权 值 的 方差 。 常 见 的 重 采样 方法 有 层次 采样 Stratified Sampling) "7, RÆK 
(Residual Sampling) 中 及 系统 采样 (Systemic Sampling) "等 。 
设 离散 随机 变量 印 的 分 布 为 概率 累积 形式 
F(x)-P(X Sum p) 


将 [0,1] < f8] 4 FX, N 个子 区 间 (Op ) | š (pepe pe) p (3 oe, 
: m-1 


Ye. 设 x 是 [0] 区 间 上 的 均匀 分 布 随机 变量 ， 根 据 xz 的 值 落 在 哪个 区 间 ， 相 应 区 


间 对 应 的 随机 变量 即 为 所 需 输出 量 。 假 设 落 在 第 j 个 区 间 ， 则 输出 为 x ， 粒 子 的 子 代 数 
n A u 的 值 落 在 该 区 间 的 次 数 。 如 图 7.3 所 示 给 出 了 重 采 样 的 实现 过 程 。 


CDF 


一 





JUR Pr(i) 
图 7.3 重 采样 过 程 图 解 
作为 例子 ， 下 面 将 给 出 系统 重 采 样 的 流程 。 
[Go wp? iO} ]-SystematicResample[ {xf wE yi] 
步骤 1: 初始 化 累积 分 布 函 数 CDF: co=0 。 
步骤 2: 构造 CDF: c,(i=1,2,---,N) . 
Ci 一 Ci-1 +u 
步骤 3: 由 CDF 的 底部 开始 启动 =1 ， 并 抽取 起 始点 四 ~Z[0.UVN] 。 
步骤 4: 沿 CDF 移动 : u =w+0-1VN (=1,2,…,N)。 
=u, >c, 时 ， i=i+] 。 
u 们 =1/V 
i-i 
必须 指出 的 是 ， 虽 然 重 采 样 在 一 定 程度 上 可 以 减轻 粒子 退化 现象 ， 但 却 因此 带 来 了 采 
样 贫 化 〈Sample Impoverishment) 问题 。 所 谓 采 样 贫 化 ， 就 是 经 重 采 样 以 后 ， 原 本 的 粒子 
集合 中 有 很 多 粒子 〈 由 于 权 值 太 小 ) 没有 “ 子 代 ” 而 少数 几 个 粒子 则 有 很 多 相同 的 “ 子 
R”, 丧失 了 粒子 的 多 样 性 。 为 此 ， 人 们 提出 了 许多 不 同 的 方法 来 解决 这 个 问题 ， 如 采用 
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重 采 样 粒子 移动 算法 (Resample-Move Algorithm) 0 、 增 加 马尔 可 夫 链 蒙 特 卡 罗 (Markov 
Chain Monte Carlo) 0?20 移 动 步 又、 对 粒子 进行 正则 (Regularization) ERS". 


732 ”重要 密度 函数 


减轻 粒子 退化 的 另 一 种 策略 是 选择 合适 的 IDF。 文 献 [14] 中 指出 ， 当 选择 的 IDF 
x(x 14,31) = PG, |x 中) 时， 可 使 权 值 的 方差 最 小 化 。 这 是 因为 ， 在 此 条 件 下 ， 有 
var, (u(? ) = (uw, y| Jee, |x )p(x, | x) dx, — p^ (y, | xt), |- 0 (7-23) 
z(x, | x5 pis) 
显然 ， 当 (xx | x, ,y,) = pGn lx 其) 时 ， 对 于 任意 给 定 的 粒子 Xp ， 无 论 怎样 从 
z(x,|x, ,, ) 进行 采样 ，w 都 有 同样 的 取 值 。 即 
w® p(y, |x, )p(x, Ix) oO PO |x, )p(x, | xy )pGQ, | x 
p(x, |x" y) T9 PO, |x,)p(x, | x£)) (7-24) 
AP, ERA =w, [p(y |x, )p(x, | )dx, 
的 计算 不 依赖 于 xÜÓ 。 需 要 说 明 的 是 ， 这 里 的 方差 指 的 是 由 不 同 的 采样 xO 所 导致 的 w 包 
的 方差 。 
最 优 〈 权 值 方差 最 小 意义 下 ) IDF ERA p(x, |x) ,y,) 进行 采样 ， 同 时 需要 计算 积 
分 项 [p(y, |x, )p(x, | xP) )dx, 。 在 通常 情况 下 ， 很 难 求 得 该 积分 项 的 解析 表达 式 ， 因 此 


实际 应 用 中 往往 采用 便于 实现 的 近似 最 优 函 数 来 代替 。 


74 ”标准 粒 于 滤波 算法 


前 面 提 到 ， 当 采用 最 优 采 样 函数 时 ， 算 法 实现 比较 困难 。 一 种 最 简单 、 常 用 的 替代 
方案 是 选择 先 验 p(xt | x ) 作为 IDF， 此 时 


(i) (i) 


w 


(o 


(x, | Xe, Yk) = p(x, | xP) (7-25) 
通过 对 该 函数 进行 采样 得 到 的 粒子 x ~ p(x, | x) ) 其 权 值 为 
wp -ub POW Hx?) (7-26) 


由 此 便 得 到 如 下 的 滤波 算法 及 图 7.4 所 示 的 实现 结构 图 。 
Alg (7.1): Standard Particle Filtering (SPF) 
(1) Bi p(xo) 得 到 个 采样 {x 中} 六 | 。 
(2) 计算 权 值 wD = p(y 1x?) 并 对 其 归 一 化 ， 即 0? =w /Dw 。 
(3) 对 粒子 进行 重 采样 ， 使 其 满足 Pr(xW” = x= ap , 
(4) 时 间 更 新 : 根据 状态 转移 函数 产生 新 的 粒子 x) ~ p(xi, | x). 
(5) 重复 步骤 (2) 一 步骤 (4)。 
最 后 得 到 k 时 刻 状 态 量 的 信 计 为 


TX 8 E (x, pod (7-27) 
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图 7.4 粒子 滤波 算法 实现 结构 


对 应 于 Alg (7.1) 中 的 处 理 流程 ， 图 7.4 给 出 了 直观 的 算法 实现 结构 。 为 便于 表述 ， 
这 里 将 通过 先 验 分 布 〈Prior) 产生 粒子 得 到 的 滤波 算法 称 为 标准 粒子 滤波 (SPF) 算法 。 

下 面 通过 一 典型 例子 来 说 明 粒 子 滤波 算法 的 工作 过 程 。 假 定 某 非 线性 函数 的 状态 方 
程 为 


x, 70.5x, ,* a +8cos[1.2(k —1)]+w, (7-28) 


2 
Xy a 





观测 模型 为 
y,7x,/20*v, (7-29) 
AP: w, 和 vw 均 为 零 均 值 高 斯 噪声 且 w, —N (0,0); v, —N (0,R) 。 该 系统 具有 明显 的 高 
度 非 线性 ， 且 似 然 函 数 呈 双 巍 特征， 这 些 特点 使 得 传统 滤波 方法 在 估计 系统 状态 时 显得 
非常 困难 。 
假设 系统 初始 状态 x,=0，Q=2，R=1 ， 根 据 上 述 系统 方程 产生 40 个 时 刻 状态 数据 
和 观测 数据 ， 如 图 7.5 和 图 7.6 所 示 。 初 始 时 刻 的 粒子 通过 高 斯 分 布 函数 产生 ， 即 
x-N(0,2), (i712,---,N) 。 采 用 N=2000 个 粒子 进行 估计 ， 所 得 结果 如 图 7.7 和 图 7.8 
所 示 。 
在 算法 实现 过 程 中 ， 假 定 有 效 粒子 数 Ns<0.5N 时 进行 重 采样 处 理 。 由 此 可 见 ， 采 
用 SMC 方法 并 结合 重 采 样 技术 可 以 得 到 构建 出 系统 的 后 验 状态 分 布 ， 从 而 得 到 系统 状 
态 量 的 有 效 估计 。 从 算法 的 实现 上 看 ， 整 个 过 程 中 采用 的 是 系统 状态 PDF 近似 的 方法 ， 
不 涉及 对 非 线性 系统 函数 本 身 的 近似 。 这 就 说 明 , 尽管 本 实例 中 采用 的 是 高 斯 系统 噪声 ， 
但 粒子 滤波 方法 适应 用 一 般 的 非 线 性 / 非 高 斯 条 件 一 一 这 正 是 粒子 滤波 算法 的 特色 所 在 。 
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Observed state y, 





0 5 10 15 20 25 30 35 40 
Time index 


图 7.5 系统 状态 序列 





System state(x,) 
° un 


Estimated p(x,ly 1x) 





Time index 





图 7.7 后 验 分 布 估 计 图 7.8 系统 状态 估计 


SPF 最 大 的 优点 在 于 算法 简单 且 易 于 实现 ， 其 缺点 是 没有 融入 新 的 观测 值 。 此 时 ， 
从 IDF 中 所 取 的 样本 与 由 真实 的 后 验 分 布 产生 的 样本 存在 较 大 的 偏差 ， 当 似 然 函数 位 于 
先 验 的 尾部 或 者 观测 模型 具有 很 高 的 精度 时 图 7.9)， 这 种 偏差 尤为 明显 中 。 这 样 ， 从 
IDF 得 到 的 大 部 分 粒子 其 权 值 都 可 以 忽略 〈 即 对 PDF 的 贡献 几乎 为 零 )， 大 量 的 计算 将 
浪费 于 此 ， 效 率 极其 低下 。 





0.3 

>... - Prior 
0.25 — Likelihood 

0.2 
0.15 

0.1 
0.05 

0 P d : 3 0 sas IM 

-8 -6 -4 -2 0 2 4 6 8 10 12 -8 $ -4 2 0 2 4 6 S8 wB 


(a) (b) 
图 7.9 ” 似 然 函数 与 先 验 位 置 关系 
为 此 ， 人 们 提出 各 种 改进 措施 ， 如 采用 正则 采样 方法 、 辅 助 粒子 滤波 采样 方法 后、 
EKF 采样 方法 和 UKF 采样 方法 的 等 ， 详 见 第 8 章 。 
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75 应 用 实例 


7.5. 雷达 制导 跟踪 
以 x-y 二 维 平面 内 的 制导 跟踪 应 用 为 例 ， 假 定 目 标 状态 为 x! =[x X y y T (k), H 
标的 运动 方程 满足 


x, 7 Fx, ,*w, (7-30) 
式 中 
EP Z 
T 为 采样 间隔 ; wx 为 目标 运动 的 过 程 噪声 ， 这 里 假定 服从 零 均值 高 斯 分 布 ， 其 协 方差 矩 
阵 为 
3 2 
o= |9 JI 2-7, 2. (7-32) 
导弹 的 运动 方程 为 
x, -Fx, ,* Gu, ,*w, 
式 中 


x, =[x” x" y” y" (k) 


a,=[c, SM ， =[c SH 
k k 


x,=x, — xr AARRE: GN to OEN to N 为 导 引 系数 ，te 为 剩余 时 间 ;， wx 为 
导弹 运动 的 过 程 噪声 ， 同 样 可 建 模 为 服从 零 均 值 高 斯 分 布 。 
传感器 位 于 弹 载 平台 观测 坐标 系 原点 ， 其 观测 量 包 括 目标 的 距离 和 方位 角 ， 观 测 方 


程 可 写 为 
| [x tk iq (7-33) 


arctan(y,/x,) | LYez 

式 中 ;距离 和 方位 观测 噪声 w， 、w ,为 非 高 斯 闪烁 噪声 。 

本 实例 中 ， 假 设 闪 烁 噪声 为 高 斯 分 布 与 拉 普 拉 斯 分 布 的 混合 形式 ， 即 

pv, )-(1- Ep (v,)*& p; V) 
-(1— E)N (V,:44,0; )-££(Y,:45,02) 

Z exp[-V2|v, - 1s |/o; ) 为 拉 普 拉 斯 函数 ， 为 闪烁 概率 。 

2 

Wu-u,70. oco,=1, oc,=5, #=02, ， 得 到 的 闪烁 噪声 序列 如 图 7.10 所 示 ， 图 中 拉 
普 拉 斯 噪声 采用 如 下 方法 得 到 ， 即 
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(7-34) 


AP: C(v,;/,o2)= 





"Oa Rn (7-35) 
式 中 : ow. ow. yA v, 为 相互 独立 的 标准 正 态 分 布 随机 数 ，cr 为 拉 普 拉 斯 噪声 标准 差 。 


amplitude 
° 


50 100 150 200 
sample index 


图 7.10 ARRAES 
仿真 中 假定 目标 初始 位 置 和 速度 分 别 为 (20,1.5)km 和 (一 100,50)m/s, 过 程 噪声 强度 为 
q'-2m/s? ; 导弹 初始 位 置 为 (40,15)km , 初始 速度 为 (一 1000, 一 150)m/s ， 为 方便 起 见 ， 不 
考虑 过 程 噪声 的 影响 。 进 一 步 假定 弹 目 总 的 交会 时 间 为 五 =6ls ， 时 间 采 样 间隔 T=0.25s ， 
导 引 系数 N,=3 , fp ie =k: T(k=1,2,---) , 7.11 给 出 了 前 60s 弹 目 接近 的 过 程 。 


15000 


axis-y(m) 





l 1:5 2 2.5 3 3.5 
axis-x(m) x104 


图 7.11 弹 目 交 会 场景 
距离 和 方位 观测 噪声 模型 满足 式 〈7-34)， 式 中 a, =20m ， c,=02°, o,-50, >» 
ao =50。， 6=0.25 。 采 用 标准 粒子 滤波 (PF) 算法 和 EKF 算法 分 别 对 目标 进行 跟踪 ， 
初始 误差 协 方差 矩阵 P=diag([50? 10? 50? 10?]) ， 粒 子 数 M=2000 。 实 验 次 数 为 200， 所 
得 结果 如 图 7.12 一 图 7.16 所 示 。 
图 7.12 给 出 了 单 次 跟踪 的 示例 〈 局 部 放大 结果 )， 图 7.13 一 图 7.16 为 两 种 算法 的 位 


置 均 方 根 误差 和 速度 均 方 根 误差 比较 。 由 图 中 的 结果 可 以 看 出 , 在 高 斯 + 拉 普 拉 斯 混合 噪 
声 条 件 下 ，PF 得 到 的 结果 要 好 于 EKF， 这 是 因为 PF 采用 的 是 概率 密度 近似 方法 ， 可 以 
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用 粒子 表征 非 高 斯 分 布 噪声 ;而 EKF 采用 的 是 矩 〈 后 验 均 值 和 方差 ) 近似 法 ， 实 例 中 采 
用 的 协 方差 矩阵 为 R=(1 一 2)Ri+eR,，( RI、R, 分 别 为 高 斯 和 拉 普 拉 斯 观测 噪声 协 方差 )， 
无 法 充分 描述 真实 噪声 的 统计 特性 ， 因 而 存在 较 大 的 估计 误差 。 另 外 ， 从 图 中 的 结果 可 
以 发 现 ， 较 之 EKF，PF 算法 在 初始 跟踪 建立 后 收敛 速度 更 快 。 这 是 因为 PF 算法 在 初始 
化 时 采用 了 大 量 的 粒子 ， 其 中 有 部 分 粒子 散布 在 目标 的 真实 状态 附近 ， 这 些 粒 子 将 占有 
较 大 的 权重 ,从 而 快速 引导 其 他 粒子 向 目标 的 真实 状态 附近 聚集 , 使 跟踪 误差 迅速 降低 。 


RMSE of position estimation 
for axis-x(m) 


RMSE of velocity estimation 
for axis-x(m) 
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图 7.12 单 次 跟踪 示例 
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图 7.13 x 轴 位 置 估计 均 方 根 误差 
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图 7.15 x #hi& 45322 RX Z 


RMSE of position estimation 


for axis-y(m) 


RMSE of velocity estimation 
for axis-y(m) 
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图 7.14 y 轴 位 置 估 计 均 方 根 误差 
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图 7.16 》 轴 速度 估计 均 方 根 误差 


两 种 算法 的 时 间 复 杂 度 见 表 7.1， 由 此 可 以 看 出 PF 的 算法 复杂 度 要 比 EKF 大 得 多 ， 
这 主要 是 因为 粒子 个 数 较 多 ， 在 实现 上 没有 做 到 完全 并 行 ， 同 时 ， 算 法 中 的 重 采样 环节 
也 会 导致 时 间 消 耗 增 大 。 


表 7.1 两 种 算法 运行 时 间 比 较 


Mean/ Cs/run) 
STD/ Cs/run) 


为 了 进一步 分 析 粒 子 数目 对 跟踪 性 能 的 影响 ， 图 7.17 给 出 了 M-1000. 2000. 4000. 
8000 4&fF F. 38 20 个 采样 时 刻 〈$s) 以 后 的 平均 均 方 根 误 差 (ARMSE)。 由 图 中 的 结 
果 可 以 看 出 ， 随 着 粒子 数目 的 增加 ， 估 计 性 能 也 相应 有 了 改善 ; 不 过 当 粒 子 数 目 大 到 一 
定 程度 以 后 (如 M>4000 )， 增 加 粒子 数 对 提高 估计 效果 的 作用 不 再 明显 ， 但 算法 的 复杂 
度 还 是 不 断 增 大 的 ， 如 图 7.18 所 示 。 因 此 ， 实 际 应 用 时 ， 应 综合 考虑 性 能 和 复杂 度 等 因 
素 ， 选 择 适中 的 粒子 规模 。 
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图 7.17 不 同 粒子 数目 条 件 下 的 跟踪 性 能 图 7.18 不 同 粒子 数目 条 件 下 的 时 间 消耗 


7.5.2 SMC-PHD 多 目标 跟踪 


在 第 6 章 曾 提 到 ， 混合 高 斯 近似 形式 为 实现 PH 滤波 的 一 条 重要 途径 ， 本 例子 中 
将 介绍 PHD 滤波 的 男 一 种 重要 途径 一 一 基于 SMC 的 实现 方法 及 在 多 目标 检测 前 跟踪 
的 应 用 。 
考虑 多 目标 应 用 场景 ， 假 设 入 ,个 目标 的 状态 演化 方程 可 表示 为 
x, = (xz...) s=1,2,-..,N, (7-36) 
AP: x; =[x; X¿ y; x Li] 为 k 时 刻 目标 s 的 状态 ， 包 含 目 标 位 置 、 速 度 和 信号 强度 等 
fal f. 表 示 一 般 的 状态 转移 函数 ; ，w, 为 过 程 噪声 。 
传感器 提供 的 二 维 监 视 区 域 (具有 n, xn, 个 分 辨 单元 ) 的 量 测 可 表示 为 
zn = XAG ef^ H, (7-37) 
W H, 
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AP: Ap) (xi) 为 观测 方程 ， 表 示 目 标 s 的 信号 强度 对 网 格 单元 (i,j) 的 贡献 ，v(i, j) A 
网 格 之 间 相 互 独立 的 观测 噪声 。 
假定 采用 点 扩展 模型 ， 目 标 强度 可 表示 为 


ij A,A, T, GA, x) * QA, - X.) 
MP xs E exp — cod (7-38) 
AP: 三 为 由 传感器 分 辩 率 引起 的 模糊 量 。 
大 时 刻 的 量 测 集 z, 及 到 时 刻 为 止 的 累积 测量 集 z 可 表示 为 
Zik ={z: l= 1,2,--.,k) (7-39) 
AH: 2 G0 :i=1,2, n; j =1,2,- sn.) e 
a. 传感器 的 似 然 函 数 可 表示 为 
TII net” |. 532) H, 
p(z, ERE ) = man (7-40) 


I IT p Cz) H, 


i=l j=l 
式 中 :，Pxw(z&7) 表 示 网 格 单元 (7 的 背景 噪声 PDF, p, GU? | xL, a) 表示 目标 + 
噪声 的 似 然 函数 。 
进一步 假设 背景 噪声 为 高 斯 噪声 ， 此 时 有 


ps. GE? | H.) = N (ze Yu D(x), a’) (7-41) 


py GE? |H,)- N G^?) (7-42) 
AP: o^ 为 噪声 方差 。 
由 于 目标 信号 仅 对 部 分 网 格 单元 产生 影响 ， 在 这 种 条 件 下 ， 似 然 函 数 p(z | x.) 可 近 
似 为 
p, | x H.) = [I II Pse GV |a)" II II Dw Gi zn) (7-43) 


ieC; (x1) jeC; (x, ) ieC, (x, ) j€C, (x, ) 
AP: d DG: jo - D,(x,) , C,(x,)= (l -gio * 4)» C;j(x,)7 Uo -4o * 43: 
(D. Ci), D, GG) = max WË) (ci) 表示 受 目标 强度 影响 最 大 的 网 格 单元 。 
TE PHD WEWER FO”, MA PHD 方程 为 
Dy. (Xk 1244) = [Du (x, | xy 4)D, (x, | 24 4) dx, + y, Q8) (7-44) 
Dyr- (€x | Xk-1) = ba Gr | Xa) + epi Srk- (X; | X (7-45) 
Xu. y.) 为 新 生 目 标 ， epa 为 目标 存活 概率 ， bya |x.) 为 从 已 存在 目标 衍生 
出 的 目标 PHD; f, ,(x, 1x.) 为 转移 概率 密度 函数 。 
在 获得 大 时 刻 的 量 测 z 之 后 ， 更 新 方程 可 表示 为 


D, |za)=| -P+ Y; Cs) _ 


‘Dipa(xXi lZ) (7-46) 
sEm aega k|k-1 EU 
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D,(z|z44) = [Bront g Dyn | zi: Xl, C7) 
AP: (x) 表示 检测 概率 ; 4 为 每 帧 数据 的 虚 警 个 数 ;， c, (z) 为 虚 警 的 分 布 函数 ; 
gx(z|xx) 为 目标 的 似 然 函 数 。 

X (7-44) ~A (7-47) 构成 了 PHD 滤波 的 递 推 过 程 ， 监 视 区 域 4 中 期 望 的 目标 
数 为 

N, = [ P, x, | z,4)dx, 

由 系统 方程 式 (7-36) 和 式 (7-37) 可 知 , 式 (7-46) 中 的 似 然 函数 可 表示 为 g (z | x,) = 
psrw (z1x)。 若 定义 似 然 比 为 1(z | x.) = ps, (z|x,)/ pa» AZ| x) g, (zx). 
在 检测 前 跟踪 (TbD ) YAH, BOTAXCRDIBS WA P (x,)=1. TJÉX (7-46) 可 等 
效 为 


, I(z | X, ) 

D g= — 3 ! (7-48) 
k (x, | Zi) P LONGE. ; ca (X, Ea) 

D (Z| ia) f I(z |x, )D,, (x, | z,4., dx, (7-49) 


在 非 线性 / 非 高 斯 条 件 下 ， 上 述 表达 式 很 难 求 得 闭 式 解 。 作 为 一 种 数值 近似 手段 ， 
SMC 方法 为 解决 这 类 问题 提供 了 一 条 有 效 的 手段 。 在 基于 SMC-PHD 的 TbD 算法 实现 
中 ， 后 验 密度 DI Q4 1) 可 由 一 组 带 权 值 Cu 的 粒子 集 {x 中} 近似 为 


Lt-l 
D, (Xk 1244) = 2. wo (Xe — xf) (7-50) 
AP: La 为 大 -1 时 刻 存 活 目标 的 粒子 数 。 
在 预测 阶段 ， 粒 子 通过 建议 分 布 gs(.| Xaia) M plela) E, B 


q,C |x, 2) ， 已 = 上 2 


un Ps | (7-51) 
pl|zx) > P= Lra tl,L a2, Li + 
式 中 : J 表示 新 生 目 标的 粒子 数 。 于 是 预测 权 值 可 通过 下 式 计算 为 
wP = u(x i)/q, (xs (Xk) P752, 7L, (7-52) 
k|k-l 一 "1 
n Gb) p Od, | Zir) p= Lea tls La +J, 


式 中 
u(Xia) = €nk-1 Gea M. ra |x) + buy a Ga | xE) 
权 值 更 新 可 通过 式 (7-48) 来 完成 ， 注 意 到 对 于 z 而 言 ， 在 计算 似 然 比 1(z|x) 时 ， 
仅 那 些 位 于 x ,附近 的 网 格 单元 才 受 其 影响 。 在 这 种 情况 下 , 权 值 的 更 新 算式 可 简化 为 
II xS 
uP = aRar (7-53) 
A, * vp) 


式 中 


Ly p Jk 
z l , l 
v2 S w, [I ieg 1x8.) 


Iz rec, xia ) seC, (s. 
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(5.5) * [D Gy D Gd )) 
由 此 得 到 大 时 刻 的 更 新 后 验 密度 为 


Dilie EuP, - x(?) (7-54) 
p=1 


在 此 基础 上 ， 对 权 值 求 和 可 得 到 目标 数目 的 估计 ， 再 利用 目标 状态 提取 方法 (如 
k-mean 聚 类 算法 ) 即 可 得 到 目标 状态 的 估计 。 但 从 算法 的 实现 过 程 看 ， 在 粒子 数目 较 大 
的 情况 下 ， 其 运算 复杂 度 是 很 大 的 ， 这 是 因为 式 〈7-53) 中 每 个 粒子 的 权重 都 要 逐 点 计 
算 。 不 过 ， 从 SMC-PHD 滤波 的 过 程 看 ， 不 难 推 知 粒子 主要 集中 于 真实 目标 所 处 的 位 置 
附近 。 这 就 意味 着 ， 由 粒子 导致 的 有 效 量 测 数 〈 源 于 目标 ) 不 会 太 多 ， 这 是 因为 大 多 数 
用 于 表征 同一 目标 状态 的 粒子 将 会 占据 近似 相同 的 网 格 单元 。 在 这 种 假设 条 件 下 ， 可 以 
得 到 一 种 高 效 的 实现 方法 ， 这 种 方法 将 所 有 的 粒子 分 割 为 P 个 子 集 ， 且 最 大 的 子 集 数目 
Pa =n, xn,。 对 于 子 集中 的 每 个 元 素 ( 粒 子 )， 由 于 具有 相同 的 权 值 更 新 公式 ， 因 此 不 
必 反 复 计 算 ， 这 一 过 程 可 用 以 下 几 个 步骤 来 完成 。 

步骤 1: 目标 位 置 抽取 

xPos(p) = xi [1], yPos(p) = xtf [3] 
步骤 2: 位 置信 息 离散 化 

iPos = round(xPos), jPos = round(yPos) 
步骤 3: 粒子 集 分 割 

SubSet[i, j] = QD. :iPos(p) = i, jPos(p) 7 j} 

经 上 述 处 理 后 ， 对 于 某 一 特定 的 包含 部 个 元 素 的 子 集 SubSet[i,, jj] 〈 该 子 集中 的 具 
ETRA (xE) h (p, € (,2,---.L, +}) )， 仅 需 进行 1 次 而 不 是 号 次 权 值 更 新 运算 ， 
从 而 可 大 大 提高 计算 效率 。 

下 面 将 以 平面 中 的 多 目标 为 例 ， 验 证 SMC-PHD 的 跟踪 效果 。 仿 真 中 假定 目标 状态 
的 演化 方程 为 

x,-Fx,,*w, s-Lh2,-,N, (7-55) 
AP: FARSHE: w, 为 零 均值 高 斯 过 程 噪声 且 协 方差 为 @ 。 
F -diag[A,F,,], Q = diag[Q,,Q,,q,T] 


1 T PA rn 
F. = , = . 
P i 各 -4 pe id 


式 中 : q =0.0001, q, 20.01 为 目标 强度 过 程 噪声 方差。 

假设 T=1ls，A, =A, =1，n, =n, =20，Z=0.7，o =1， 利 用 这 些 参数 设置 产生 
25 帧 图 像 数 据 。 初 始 时 刻 ， 监 视 区 域内 没有 目标 ， 在 第 6 秒 和 第 11 秒 分 别 出 现 一 新 目 
标 ， 而 在 第 21 秒 时 第 1 个 目标 消失 ， 另 一 个 目标 一 直 存 在 直到 第 25 秒 。 在 整个 过 程 中 ， 
暂 不 考虑 衍生 目标 。 

目标 1 和 目标 2 的 初始 状态 分 别 为 [4.2 0.1 7.2 0.1 刀 和 [0.0 0.1 8.2 0.12 7], SMC-PHD 
算法 中 的 参数 设置 见 表 7.2， 目 标的 初始 强度 估计 在 L-5 到 1-5 之 间 均 匀 采 样 。 
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X72 SMC-PHD 滤波 算法 参数 设置 
| 
| sas | oo | os | o | 1 | 2 | “o | 3 | 

假设 目标 的 初始 强度 分 别 为 T=30,20,15,10 ， 对 应 的 信 噪 比 SNR 分 别 为 19.8、16.3、 
13.8 和 10.24B， 其 计算 公式 为 SNR = 201g[/A,A,/QnZ/o)] 。 图 7.19 给 出 了 目标 的 真实 


航 迹 ， 图 7.20 为 不 同 强度 下 得 到 的 第 15 帧 图 像 。 
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图 7.19 目标 航 迹 示例 
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图 7.20 目标 强度 图 像 
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由 图 7.20 中 的 结果 可 以 看 出 ， 背 景 品 声 中 的 目标 是 较为 微弱 的 ， 即 便 是 在 7=30 的 
情况 下 ， 仍 存在 大 量 的 虚 警 ， 此 时 采用 传统 先 检 测 后 跟踪 的 方法 很 可 能 丢失 目标 ， 很 难 
得 到 理想 的 效果 。 为 此 这 里 主要 考虑 基于 SMC-PHD 的 检测 前 跟踪 (ToD) 方法 。 

进一步 假设 目标 初始 强度 I=30 ， 采 用 SMC-PHD 算法 进行 跟踪 。 对 于 多 目标 跟踪 ， 
不 仅 要 估计 目标 数目 ， 还 要 估计 各 目标 的 状态 ， 此 时 单 目 标 应 用 条 件 下 的 性 能 测度 方法 
己 不 能 满足 要 求 。 为 此 ， 这 里 采用 最 优 子 模式 分 配 (Optimal Sub-Pattern Assignment, 
OSPA) 四 距离 指标 进行 测度 ， 其 定义 为 


lp 
dp ax Amie 2:45 (xo) +e,- ») uius 
nA EP tal 


RP: X IX CAIEIECTÓER, WORD m fln, Bm<n; 1X pco, dO (x 
min (c,d(x,£)); c>0, d(xX|x-x|: 表示 {1,2…,k} 的 所 有 排列 组 成 的 集合 。 若 
m»n, WJS dC (x X)-dO (K X), 
对 于 多 次 仿真 ， 则 需 采用 OSPA 的 均 方 根 形 式 ， 其 定义 为 
1/2 
d, o Ew] (7-57) 


式 中 : M 为 实验 次 数 ，d (Lk) 为 第 i 次 实验 的 OPSA 距离 。 

采用 随机 产生 粒子 进行 初始 化 的 方法 进行 跟踪 ， 即 目标 位 置 在 监视 区 域 
[0 20]x[0 20] 内 均匀 取 值 ， 而 目标 速度 则 在 [0 0.2]x[0 0.2] 范围 内 均匀 取 值 ， 总 的 粒子 数 
H W=10000 ， 其 中 用 于 新 生 目 标 检测 的 粒子 数目 为 2000。 取 c=20 ，p=2， 仿 真 次 数 为 
M=100 ， 所 得 结果 如 图 7.21 一 图 7.23 所 示 。 
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图 7.21 目标 位 置 估计 结果 


由 图 7.21 一 图 7.23 中 的 结果 可 以 看 出 ， 采 用 SMC-PHD 跟踪 算法 ， 可 实现 对 可 变数 
目 条 件 下 的 多 目标 检测 前 跟踪 ， 且 对 目标 数目 的 估计 较为 准确 、 可 靠 。 从 图 7.23 的 结果 
看 ， 当 目标 数目 发 生变 化 的 时 (尤其 是 在 有 新 生 目 标 出 现时 )，OSPA 距离 会 出 现 增 大 的 
现象 , 这 与 目标 数目 发 生变 化 时 将 引入 新 的 误差 的 事实 是 一 致 的 。 随 着 跟踪 过 程 的 持续 ， 
目标 状态 的 估计 逐渐 趋 于 稳定 ， 相 应 的 OSPA 距离 也 随 之 降低 。 
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图 7.22 目标 数目 估计 均值 A 7.23 OSPA 距离 均 方 根 


需要 指出 的 是 ， 本 实例 中 考虑 的 是 在 无 任何 先 验 信息 条 件 下 的 粒子 初始 化 方法 〈 在 
整个 监视 区 域 随机 均匀 分 布 ); 这 种 通用 设置 能 适应 复杂 的 跟踪 条 件 , 但 相应 地 粒子 的 效 
率 也 较 低 ， 同 时 也 影响 了 跟踪 性 能 的 提高 。 另 外 ， 本 实例 中 考虑 的 目标 场景 相对 较为 简 
H, 对 于 具有 多 个 临近 目标 或 目标 存在 交叉 航 迹 等 复杂 情况 ， 需 要 进行 一 些 特殊 的 处 理 ， 
感 兴趣 的 读者 可 参阅 相关 文献 如 文献 [29] 等 。 
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本 章 主要 阐述 基于 非 参 数 化 蒙特 卡 罗 仿 真 技术 来 实现 递 推 贝 叶 斯 滤波 的 原理 和 实现 
方法 ， 首 先 从 贝 叶 斯 滤波 问题 中 多 维 积分 式 的 求解 入 手 ， 阅 述 了 随机 采样 近似 原理 和 基 
于 重要 采样 技术 的 序 贯 处 理 方法 ， 在 此 基础 上 针对 采样 函数 为 系统 先 验 〈 状 态 转移 ) HR 
数 的 一 类 特殊 情况 ， 重 点 阐述 了 标准 粒子 滤波 (SPF) 算法 的 具体 处 理 流程 ， 并 给 出 了 
制导 雷达 跟踪 和 多 目标 检测 前 跟踪 的 应 用 实例 。 

实验 结果 表明 ， 与 传统 的 解析 高 斯 近似 方法 相 比 ， 以 随机 采样 近似 为 基础 的 粒子 滤 
波 算法 由 于 具有 粒子 散布 性 的 特点 ， 在 提高 跟踪 系统 的 稳定 性 和 收敛 速度 方面 具有 明显 
的 优势 。 另 外 ， 作 为 一 种 通用 的 贝 叶 斯 推演 手段 ， 粒 子 滤波 不 仅 能 够 处 理 传统 高 斯 型 滤 
波 方法 所 能 处 理 的 非 线 性 估计 问题 ， 还 能 处 理 非 高 斯 、 多 模 态 条 件 下 的 复杂 问题 ， 具 有 
更 广泛 的 适应 性 和 更 巨大 的 应 用 潜能 。 

需要 指出 的 是 ， 粒 子 滤波 的 优势 在 于 其 通用 性 ， 这 种 优势 不 是 针对 任何 非 线性 滤波 
问题 都 具备 的 ， 这 是 因为 : 叫 作为 一 种 基于 随机 采样 的 实现 方法 ， 粒 子 滤波 算法 必须 以 
大 量 的 粒子 为 基础 ， 运 算 开 销 较 大 ;对 于 某 些 简 单条 件 下 的 非 线 性 滤波 问题 ， 如 基于 
目标 距离 、 方 位 量 测 的 跟踪 ， 传 统 的 解析 高 斯 近似 方法 本 身 己 能 达到 较 理想 的 效果 ， 采 
用 粒子 滤波 手段 所 能 获得 的 额外 效果 其 微 。 因 此 ， 在 实际 应 用 中 ， 应 该 根据 待 处 理 问题 
的 特点 对 拟 采 用 的 方法 进行 合理 选择 。 
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第 8 章 ”优选 分 布 采样 滤波 


从 第 7 章 的 分 析 知 道 ，SPF 直接 根据 状态 转移 方程 (或 先 验 分 布 ) 产生 新 的 粒子 ， 
由 于 没有 考虑 新 近 观 测 对 状态 估计 的 影响 〈 即 粒子 的 采样 完全 脱离 了 观测 信息 的 指导 )， 
这 种 滤波 器 在 很 多 情况 下 表现 出 较 差 的 性 能 ， 即 便 在 粒子 数目 很 大 的 情况 下 也 是 如 此 。 

为 了 提高 粒子 滤波 算法 的 估计 性 能 ， 各 种 基于 重要 性 函数 采样 的 粒子 滤波 方法 应 运 
而 生 , 这 类 方法 的 共同 点 是 在 产生 粒子 时 融入 了 观测 信息 ,因而 所 得 到 的 粒子 更 加 有 效 ; 
不 同 点 在 于 指导 粒子 产生 的 方法 存在 差异 ， 即 重要 采样 函数 (或 建议 分 布 ) 的 选择 不 同 。 
理论 上 ， 为 了 最 大 限度 地 提高 粒子 的 质量 和 效能 ， 希 望 得 到 用 于 指导 粒子 生成 的 最 优 采 
样 函数 。 但 是 对 于 一 般 的 非 线性 系统 ， 最 优 重要 采样 函数 很 难 求 得 或 根本 不 存在 ， 只 能 
通过 近似 的 方法 来 获取 次 优 的 建议 分 布 。 

本 章 主要 研究 和 分 析 几 种 基于 重要 采样 函数 的 粒子 滤波 算法 ， 包 括 最 优 采样 函数 近 
似 滤波 粒子 算法 、EKF 近似 粒子 算法 、UPF 及 其 修正 算法 等 ， 针 对 粒子 退化 问题 ， 介 绍 
了 用 于 保持 粒子 多 样 性 的 马尔 可 夫 链 蒙特 卡 罗 (MCMC) 重 采 样 方法 ， 并 结合 目标 跟踪 
应 用 给 出 了 两 个 典型 的 实例 。 


8.1 Be CERE UC FEAR UE 


8.1.1 最 优 重要 函数 近似 
在 前 面 的 讨论 中 提 到 ， 选 择 合适 的 重要 函数 可 以 有 效 减 小 权 值 的 方差 ， 从 而 减轻 粒 

子 退化 问题 。 通 常情 况 下 ， 对 于 一 般 的 非 线性 系统 
Xk. = / (x, ) + W, 


y, = h(x,) + v, 
很 难得 到 最 优 重要 函数 p(x | x, Ly.) 的 解析 表达 解 ， 但 若 系统 方程 满足 如 下 形式 


(8-1) 
(8-2) 


Xen = f(x, )+ w, w, ~ N (0,0,) (8-3) 

y, = H,x, * v, v, ~ N (0, R,) (8-4) 
可 以 证 明 〈 见 附录 E)， 最 优 采 样 函数 的 解析 表达 式 为 

p(x, |x, Y1) = N (x, ; m, P.) (8-5) 


式 中 
m, = (Qj f(x) * HER y), P;'=Q, * HER, H, 

对 于 式 〈8-2) 所 表示 的 一 般 非 线性 观测 ， 虽 然 无 法 直接 求 得 最 优 解 ， 但 可 通过 对 
模型 进行 局 部 线性 化 处 理 ， 求 得 近似 最 优 解 。 特 别 地 ， 若 对 式 (8-2) 进行 一 阶 泰勒 展 
JF, 得 
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y, & h(f KD) + H,[x, — f(x, a)] + v, (8-6) 


式 中 
H, mV AGO, reo 
对 照 式 (8-4)， 易 知 此 时 有 
m, = P, (Q; f (asa )* HR, [y, - hCf (x, 4) + H, f(x, 4] (8-7) 
B, -(Q;! + H; Re H,)" (8-8) 


即 近似 最 优 采样 函数 为 Ax | x Y) = AN (0, B)» 于 是 可 通过 该 采样 函数 引导 
粒子 的 生成 。 


8.1.2 MCMC 重 采 样 


由 Alg (7.1) ff] (3) 可知, 重 采样 是 根据 计算 所 得 的 权重 对 粒子 或 舍弃 或 多 份 复制 ， 
这 一 过 程 引入 的 一 个 负面 影响 是 当 大 多 数 粒子 的 权重 较 小 而 只 有 少数 〈 极 端 情况 下 只 有 
1 个 ) 粒子 占有 绝对 性 的 权重 时 ， 经 重 采 样 后 将 得 到 大 量 完全 相同 的 粒子 ， 粒 子 的 多 样 
性 丧失 ， 导 致 粒子 贫 化 问题 。 显 然 ， 在 这 种 情况 下 ， 极 其 少量 的 有 效 粒 子 很 难 对 真实 的 
状态 分 布 进行 准确 的 描述 ， 算 法 性 能 将 会 严重 恶化 。 文 献 [30] 指 出 ， 通 过 某 种 马尔 可 夫 
链 进行 蒙特 卡 罗 积 分 运算 ， 可 以 保持 状态 分 布 不 变 。 为 此 ， 可 以 借助 这 一 思想 ， 在 重 采 
样 之 后 引入 MCMC 步骤 ， 使 粒子 向 所 期 望 的 状态 空间 移动 ， 在 恢复 粒子 多 样 性 的 同时 
不 改变 原 有 的 分 布 。 其 基本 思路 为 : 在 获得 重 采 样 的 粒子 si 后 ， 通 过 马尔 可 夫 链 转移 
Ei v (xi? | ED) 产生 一 组 新 的 粒子 xL 使 之 满足 


f p y (xi | apar | a s Vik jaz” = px? | x a) (8-9) 


通常 ， 上 述 不 变性 条 件 可 通过 Metropolis-Hastings 算法 来 实现 ， 其 基本 流程 可 概括 
如 下 。 

Alg (8.12: Metropolis-Hastings Algorithm 

(1) 根据 当前 时 刻 丰 得 到 的 重 采 样 索 引 7， 用 xDD 取代 xi), 。 

(2) 对 每 一 个 粒子 xO) ， 通 过 式 〈8-7) 和 式 (8-8) HE AO M ÊO 。 

(3) 对 IDF 进行 采样 : xi? ~ N (x,; mi, BO). 

(4) 产生 满足 均匀 分 布 的 随机 数 a~ U(0,1) 。 

(5) 拒绝 /接受 。 


Zasmin(lLw(x/))/u(xP)), xP 2x0; tW, x?-xP. 
8.3 滤波 算法 流程 
综合 以 上 分 析 ， 改 进 的 粒子 滤波 算法 〈 这 里 称 为 IPF) 完整 的 计算 流程 如 下 已 ' 321, 
Alg (8.2): Improved Particle Filtering (IPF) 
(1) 由 先 验 分 布 p(x。) 得 到 N 个 采样 Go s ° 


(2) 根据 式 (8-7) FIR (8-8) HAA. PO 493189 AP ~ N Qu mp ÊP). 
G) HE ut < up, POLEP XE 并 归 一 化 得 0 。 
(007 NGP -mp, BP) 


127 


(4) 对 粒子 (30), 进行 重 采样 得 到 一 组 新 粒子 {四} 六， ， 使 其 满足 Prat = x0) 
=ë? 。 

(5) MCMC 步骤 。 

(6) 重复 步骤 (2) 一 步骤 CS). 

与 标准 粒子 滤波 (SPF) 相 比 ， 改 进 的 算法 OPF) 在 采样 函数 中 融入 了 新 近 的 量 测 ， 
能 使 产生 的 粒子 更 好 地 近似 真实 的 状态 分 布 ; 同时 , MCMC 过 程 能 有 效 防止 粒子 贫 化 现 
象 的 发 生 ， 从 而 提高 算法 的 性 能 。 


82 EKF 近似 粒子 滤波 


在 8.1 节 中 ， 围 绕 最 优 采 样 函 数 p(x, | x, ,, y,) 的 近似 问题 ， 介 绍 了 线性 化 展开 近似 
的 处 理 方法 。 ØRE, plx, | x, 4, y,) 表达 的 是 利用 kk 一 1 HARE x, 和 kk 时 刻 量 测 信息 
PAREL k TZ x, 的 最 优 估计 问题 ， 通 常情 况 下 px, |x, aL y) 的 解析 表达 式 并 不 存在 ， 
实际 应 用 中 ,为 了 处 理 问题 的 方便 ， 常 将 其 近似 为 高 斯 分 布 。 由 于 EKF 提供 了 近似 求解 
p(x, |x, 4, ,) 的 一 条 重要 的 途径 ， 自 然 地 可 将 其 与 传统 的 粒子 滤波 结合 得 到 EPF 
(Extended Particle Filtering) 算法 。 接 下 来 将 详细 介绍 该 算法 的 原理 和 实现 方法 。 

在 EPF 算法 中 , 通过 kk 一 1 时刻 的 N 个 粒子 {x wO Y" RIFH EKF 更 新 方法 得 到 kk 时 
刻 的 入 个 粒子 集 {x8 ,wn}7-!， 对 每 一 个 粒子 xi ， 上 述 过 程 可 表示 为 

p(x, 1x4: y) = Ty (x, | ViN (x, JA 

实际 处 理 时 ， 可 采用 N 个 并 行 的 EKF 滤波 器 组 来 实现 ， 具 体 算法 流程 可 概括 如 下 。 

Alg (8.3): Extended Particle Filtering (EPF) 

WR: 初始 化 : k=0 

从 先 验 状态 分 布 p(xo) =N (x; XD, PO) EIU AURUT. P o 


For kz1,2,-- 

步骤 2: 重要 采样 过 各 

For iz1,2,-.,N 

(1) 用 EKF 对 每 一 粒子 进行 状态 更 新 。 

时 间 更 新 : 
= (8-10) 
py, = FP PE (FEY +Q, (8-11) 

AP: F? = Vf), so. 

测量 更 新 : 
Rem (BY [HE (PY -RT' (8-12) 
$0 s< 18 | * K,(y, 一 AL) (8-13) 
PP = rp. -EP HPP, (8-14) 


式 中 : HP =V,h(x)|,_ FN 


128 


(2) 从 IDF 进行 采样 。 
x ~ Tv X, 1314) 7 AN Qn; Xp, P?) (8-15) 
For i=1,2,…,N ， 估 算 每 一 粒子 的 权重 并 进行 归 一 化 : 


uj) = WPD Exp )pGs | x)/ zv (xÉ | yx) (3-16) 
P - IY" at () (8-17) 
步骤 3: 重 采样 过 程 
对 粒子 (x0), 进行 采样 (选择 ) 得 到 N 个 新 的 样本 (xO) ， 并 使 其 概率 满足 
Pr(xO = xO) = d (8-18) 
步骤 4， 估 计 输 出 
&, = E[x, | yu] Dox (8-19) 


通过 执行 步骤 1 一 步骤 3 的 递 推 步骤 ， 可 以 序 贯 得 到 状态 量 的 估计 。 


83 UPF 算法 及 其 修正 


在 前 面 的 分 析 中 指出 ， 粒 子 滤波 的 关键 是 如 何 产生 有 效 的 采样 来 近似 后 验 PDF， 实 
质 上 就 是 寻求 便于 实现 且 又 能 较 好 近似 真实 分 布 的 IDF。 鉴于 UKF 算法 本 身 能 够 有 效 融 
合 观测 信息 ， 并 将 粒子 推 向 高 似 然 的 区 域 由 新 近 的 观测 量 y, 来 实现 )， 从 而 对 真实 的 
PDF 进行 有 效 近 似 ， 文 献 [24] 中 提出 了 用 UKF 的 滤波 结果 作为 建议 分 布 的 思想 。 由 于 该 
方法 比 直 接 用 先 验 或 由 EKF 产生 建议 分 布 的 算法 性 能 都 好 , 因此 在 很 多 应 用 场合 受到 重 
视 B333。 但 是 ， 在 实际 应 用 中 发 现 ，UPF 同样 存在 潜在 的 不 足 ， 特 别 是 在 过 程 噪声 协 方 
25 Q, 与 估计 误差 协 方差 忆 相差 悬殊 的 情况 下 极 易 出 现 数值 敏感 问题 ， 并 由 此 导致 严重 
的 粒子 贫 化 现象 。 在 许多 应 用 特别 是 被 动 目 标 跟 踪 应 用 中 ， 由 于 跟踪 误差 一 般 都 较 大 ， 
而 过 程 噪声 通常 要 小 得 多 ， 因 此 @, 与 P, 相差 悬殊 是 一 种 较为 普遍 的 现象 。 有 鉴于 此 ， 
本 节 将 在 传统 UPF 的 基础 上 提出 一 种 修正 算法 (MUPF ), 该 算法 通过 修正 权 值 的 计算 避 
免 了 数值 敏感 问题 ， 并 改善 了 权 值 的 分 布 特性 〈 即 降低 权 方 差 )， 即 便 在 Q, 奇异 的 情况 
下 也 能 进行 方便 的 计算 。 

在 接 下 来 的 几 节 中 ,首先 将 对 传统 UPF 算法 作 一 简要 回顾 ， 在 此 基础 上 讨论 数值 敏 
感 问 题 如 何 影响 滤波 器 的 性 能 ， 同 时 提出 一 种 修正 算法 来 有 效 解决 UPF 中 存在 的 不 足 。 


8.3.1 UPF 算法 


UPF 算法 的 基本 思想 是 采用 UKF 为 每 一 粒子 产生 所 需 的 IDF z (x, lY) ， 以 此 来 
近似 最 优 采 样 函 数 ， 即 
p, |x, yk) * mv (x, | yu.) (8-20) 
通过 对 这 一 分 布 函数 进行 采样 可 得 到 新 的 粒子 。 关于 UPF 算法 的 详细 推导 可 参阅 文 
献 [24]， 这 里 仅 给 出 算法 的 计算 流程 。 
Alg (8.4): Unscented Particle Filtering (UPF) 
步骤 1: 初始 化 : k=0 
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从 先 验 状态 分 布 p(x,) = N (xo; Xy, P) 抽取 一 组 粒子 (xO ， 同 时 定义 一 组 参数 
co -kKL*k), cj=cr =0.5/(L+x)，y=VL+xk 。 其 中 j=1,2,…,L， 工 为 状态 x 的 


维 数 ，y 为 尺度 因子 。 
For k=1,2,:::- 
步骤 2: 重要 采样 过 程 
For izL2,-.,N 
(1) 用 UKF 对 每 一 粒子 进行 状态 更 新 。 


产生 变换 样 点 : 
Lo x" s 
3E Los; -[££^, ip +£ P? 2p, -Y4 P] 
时 间 更 新 : 
(i) - (i) 
Mkit-lo2r = J oor) 
(i) < (i) 
^ i 
Xj = Ze NE 
j-0 
(i) ~ (i) (i) () (i) yT 
1 1 ^U i A (i 
Puka = > c, Qu; 7 Xie Xia) + Q, 
j-0 
*(i) == (i) (i) (i) 
Akik-1,02L, = |a Xuko YVO: — Xiao - AQ, ] 
*(i *(i 
PETS T hw ioar, ) 
2L, 
a(i) _ *q a(i) 
Jura m S4MD. 
j-0 
测量 更 新 : 
2L, 
- # *(i) a(i) *(i) AU) XT 
Pn E > Qiu; =R kik- X34, Ji T R, 
j= 
2L, 
= * t ap) a(i) *(i) AG) yT 
Pan = 2.6; Qiu — Xie Xe 7 Jie) 
J” 
- 
K, = Pn Pn 
a(i) .. A) a(i) 
X, = Sik- +K (Ik - 95-0 
(i) _ p») T 
Pe = Pika K,P,, y, K, 
式 中 


L,=2L, c -klL,*k). c; =c, =05/(L,+x) 


(2) M IDF 进行 采样 。 
xÉ zy, |p.) = N(x,; , RO) 
For iz12,-.,N 
估算 每 一 粒子 的 权重 并 进行 归 一 化 : 
wh =wh p(y | x )p(xP | xt)! zy Gi? | pa) 


~(i) 2) N (D 
Up wp, uw, 
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(8-21) 


(8-22) 


(8-23) 


(8-24) 


(8-25) 
(8-26) 


(8-27) 


(8-28) 


(8-29) 


(8-30) 
(8-31) 
(8-32) 


(8-33) 


(8-34) 
(8-35) 


对 粒子 (PY, 进行 采样 〈 选 择 ) 得 到 N 个 新 的 样本 (xD } 台 ， 并 使 其 概率 满足 
Pr(x() = x?) = 0” (8-36) 
步骤 4， 估 计 输 出 
& = Elx, | yu] 277 Up x? (8-37) 
通过 执行 步骤 1 一 步骤 3 的 递 推 步骤 ， 即 可 得 到 状态 量 的 估计 。 
8.3.0 ”修正 的 UPF 算法 
UPF 算法 (通过 UKF 生成 建议 分 布 ) 被 认为 是 最 有 效 的 估计 方法 之 一 ， 但 在 实际 应 
用 中 发 现存 在 这 样 几 个 问题 : @ 当 协 方差 矩阵 Q, 奇异 时 ， 式 (8-34) 中 的 p(x | x) it 
算 较 为 困难 ，@@ 当 状态 估计 误差 协 方差 矩阵 严 和 过 程 噪声 协 方差 矩阵 Q, 在 量 值 上 不 可 比 
拟 〈 这 在 实际 问题 中 非常 普遍 ) 时 ， 数 值 敏 感性 问题 〈 即 计算 溢出 ) 就 会 随 之 产生 ; OH 
于 @， 算 法 将 会 遭受 严重 的 粒子 退化 问题 ， 这 是 因为 在 每 次 递 推 过 程 中 ， 绝 大 多 数 粒 子 的 
权 值 小 到 可 以 忽略 ， 仅 有 个 别 的 粒子 占有 较 大 的 权重 。 下 面 以 图 8.1 所 示 的 简单 二 维 状态 
估计 问题 为 例 ， 解 释 这 种 现象 出 现 的 原因 。 


X = — Mr; En PL) 
N 





图 8.1 二 维 状态 空间 中 的 先 验 计算 示例 
如 图 8.1 所 示 , 在 k 一 1 时刻， 状态 分 布 满足 x~NN(xi_1; 训 1, 忆 1) ; 当 经 过 系统 模型 
传递 之 后 ， 其 分 布 可 记 为 p(x, lia) = N (x, - /(X,450,9,) 。 为 讨论 方便 ， 这 里 假设 和 矩 
阵 @, 为 对 角 阵 且 Q, = diag{o?,o2}。 在 假设 条 件 下 , 显然 状态 预测 误差 e, = x, — f(&, D) 
在 两 轴 长 度 分 别 为 o, 和 的 椭圆 体 所 包含 的 范围 之 内 。 但 是 ， 经 过 测量 更 新 并 通过 重 
要 函数 采样 (R (8-33)) ZJE, HFR (8-33) 引入 了 较 大 的 采样 不 确定 性 ，d = [x - 
f (X, )|#l o, =max(o,,o,) 的 量 值 很 可 能 不 在 同一 数量 级 。 一 方面 ， 这 会 引起 数值 敏 
感 问题 ， 另 一 方面 ， 老 忽略 所 有 的 非 显 著 性 权 值 ， 则 会 进一步 导致 粒子 退化 问题 。 为 详 
细 说 明 这 一 点 ， 首 先 将 状态 误差 表示 为 
eP =x f(s) =A Ag 了 ian 
进一步 有 
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(e) QF ek = aj (AxA) + o; (Ar 
> o; Ax) + (Ax) ]= (o dy 
HixX (8-39) 可 以 清楚 地 知道 ， 若 dO > ama 成 立 ， 则 状态 先 验 
p(x | xt) )= const -exp[-0.5 (e? )' Q; e] — 0 (8-40) 
事实 上 ， 这 一 现象 是 非常 普遍 的 ， 这 是 因为 通常 情况 下 过 程 噪声 相对 较 小 ， 而 状态 
估计 误差 可 能 会 很 大 ， 相 应 地 其 协 方差 为 了 能 包含 估计 误差 也 将 会 非常 大 。 
通过 上 述 分 析 可 知 ， 问 题 的 根源 在 于 式 〈8-34) 中 的 重要 权 计算 。 为 避免 煞 值 敏感 
问题 ， 一 种 可 能 的 方法 是 先 计算 对 数 权 ， 而 后 通过 忽略 共同 的 比例 因子 得 到 归 一 化 的 权 
值 ， 这 种 方法 在 某 些 情况 下 可 能 奏效 。 但 是 ， 一 方面 ， 若 dt? e[du, du] 存在 于 一 个 很 
大 的 数值 区 间 ， 数 值 敏感 性 问题 仍然 不 能 解决 ， 也 就 无 法 得 到 粒子 的 权重 ; 另 一 方面 ， 
即便 在 某 些 情况 下 通过 计算 对 数 权 可 以 消除 数值 敏感 问题 ， 但 由 于 权 值 分 布 的 极端 不 均 
匀 性 ， 仍 会 不 可 避免 地 出 现 严重 的 粒子 退化 问题 。 接 下 来 ， 将 提出 一 种 修正 形式 的 权 值 
计算 方法 中 I， 来 有 效 解决 前 述 的 几 点 不 足 。 
实际 上 , 在 UPF 框架 下 ,状态 的 后 验 分 布 是 用 混合 高 斯 模型 近似 的 ， 即 在 一 1 时 刻 ， 有 
P(X | Vix) = k. p N (x, i; Pr) (8-41) 


为 简便 起 见 ， 这 里 将 以 单个 粒子 为 例 ， 讨 论 其 状态 更 新 过 程 ， 其 基本 目标 是 由 
PG a | Ya) 的 第 i 个 分 量 得 到 后 验 分 布 px? | p) 的 估计 。 根 据 贝 叶 斯 规则 ， 预 测 分 
布 可 计算 为 


(8-39) 


pod? | Jia) = [POL 1x vu Dp | Paadi, 
= |V(xP; SPPE, | y, dx? 
在 UKF 算法 中 ， 隐 含 假定 了 预测 分 布 是 高 斯 型 的 ， 即 px? | yu.) 可 近似 为 
pf? | yu) = N (xP; Sas Pe) (8-43) 
其 中 ， 状 态 预 测 及 相应 的 协 方差 分 别 为 
30 = Elxf | yaa ]= |x pG | ya ado 
= [| Pp reds fos. Q x? | PG 1 ext? (8-44) 
= JAPPE, | pax, = [FEN Cx; BO), 


| ) ali ) a) 4T 
P, m i * E[(x£) - £i Xx” E £5 1) ] 


(8-42) 


(i) e) (i eG) qT (i) . G) (i) (i) (8-45) 
=Q, + [n (x5) 7 Xy alf) i-i] N (Xi; Ch, Pr dx, 
同样 ， 有 
Pik- = E[l y® Jx, PA = [DP po? bh Tm n 
(8-46) 


i i i i i i). ZG i i 
= [APPP |x, ya Xx? = APN Qr t a P xD 
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= E[Cy? = i X», 一 38i 11 


dele | E n (8-47) 
=R, + [AL - yl URP) - $l T N GP: R ,, PR dx? 
Piy = EIR — XS) — Pika) 1 EN 
= [e - 359 GP) - 39" N GP: 3p BIO dx 
因此 
KE a PO mm y (8-49) 
XD = iiki + KP) (y, — Jg (8-50) 
PP = Pia -KP PO (Ki). (8-51) 


xX (8-44) —3X (8-48) 所 示 的 解析 表达 式 是 为 了 说 明 问 题 的 求解 方法 。 在 实际 应 
用 中 ， 这 些 式 子 的 计算 仍 采 用 与 式 (8-23) 一 式 〈8-29) 相似 的 UT 方法 来 实现 。 如 此 一 
来 ， 重 要 权 的 计算 式 可 表示 为 

"m px? | ia) - p [xf pia | iaa) 
Ty (xP | Jia) Ty GP | Yi) 
由 此 可 知 ， 当 从 建议 分 布 zv(xo | yu) = A (o; XO, PO) 进行 采样 得 到 粒子 x 之 
后 ， 相 应 的 权 值 为 
0 < PO | XE) p(x Via) POn Ex) Qao Pika) 
E N (xp; te Pj?) N (x; &p, pf?) 
在 得 到 k 时 刻 所 有 粒子 的 权 值 之 后 ， 该 时 刻 总 的 后 验 概率 密度 可 计算 为 
px, |p) DPN Q5 Rp PO?) (8-54) 
AFP: o 为 归 一 化 权重 。 

几 点 说 明 : 

(1) 这 里 所 提 的 修正 的 UPF (MUPF) 算法 可 视 为 重要 采样 方法 和 高 斯 混合 近似 的 
结合 ， 该 方法 与 文献 [37] 一 文献 [39] 中 的 高 斯 和 滤波 器 (GSF) 有 明显 的 不 同 。 在 GSF 
中 ， 每 一 个 高 斯 分 量 通过 EKF 来 更 新 ， 且 相应 的 权 值 单纯 由 似 然 计 算得 到 ， 即 w(? = 
N (y, — (Sin -i);0, PR) - 

(2) 重要 采样 方法 的 应 用 实例 在 文献 [40] 提 出 的 高 斯 和 粒子 滤波 器 (GSPF) 中 已 有 
介绍 ， 但 这 里 所 提 的 MUPF 算法 与 GSPF 仍 有 着 本 质 的 差异 。 在 GSPF 中 ， 每 一 个 高 斯 
分 量 的 预测 均值 和 协 方差 是 通过 蒙特 卡 罗 仿 真 的 方法 得 到 的 ， 这 意味 着 需要 产生 大 量 的 
粒子 来 计算 状态 均值 和 方差 。 与 此 不 同 的 是 , 对 于 UPF 和 MUPF 而 言 ， 前 二 阶 矩 的 更 新 
是 通过 UKF 算法 来 实现 的 。 粒子 从 混合 高 斯 分 布 进行 采样 ， 且 每 个 高 斯 分 量 仅 产 生 一 个 
RU. 

(3) ÆA (8-34) 中 ， 权 值 通 过 先 验 计算 递 推 求 得 ， 与 此 相 比 ， 式 (8-53) 中 的 修 
正 权 值 则 通过 预测 分 布 进行 估算 。 由 于 预测 协 方差 和 滤波 协 方差 是 可 比拟 的 ( 即 在 量 值 
上 相近 )， 故 数值 敏感 问题 将 得 以 消除 ， 粒子 权 值 的 分 布 特性 也 同时 得 以 改善 。 这 反 过 来 
可 以 有 效 避 免 粒子 退化 问题 ， 从 而 大 大 提高 滤波 器 的 估计 性 能 。 


(8-52) 


(8-53) 
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8.4 应 用 实例 


8.4.1 IPF 的 只 测 角 跟 踪 
考虑 二 维 平面 内 的 只 测 角 跟 踪 (BOT) 问题 目 ， 目 标 运动 的 状态 方程 满足 


X4 FI, +W. (8-55) 
AP: 状态 x =[x, X, y, x] : BEEF = diag(F,,F,); 7 为 观测 时 间 间 隔 ，w, 为 扰动 噪 
声 ， 其 协 方差 阵 0, = diag{Q@ ,0 } - ^h. F, -| 4 o, -| pa 
0 1 pyuz T 
观测 量 为 目标 方位 角 ， 其 观测 方程 为 
P, =arctan(x /y,)+v, > k=1,2,.…,K (8-56) 
AP: v, 为 零 均值 高 斯 噪声 ， 即 vy, ~ MV (0,02) 。 
仿真 中 取 T-1s, q=0.001, o =0.005 。 目 标 初始 状态 为 x, =[—0.05,0.001,0.7, 
一 0.055] ， 天 = 40 ， 状 态 先 验 分 布 p(xo) = N (x; P); HF x, =[-0.06,0.0015,0.65， 
— 0.05]! , P, = diag(0.022,0.0012,0.052,0.012) 。 目 标 跟踪 的 效果 用 航 迹 的 均 方 误差 (MSE) 
来 度量 ， 其 定义 为 


1 " x 
MSE = 二 Dl -名 + 7 Y] (8-57) 


图 8.2 为 几 种 不 同 算 法 (EKF、UKF、SPF 和 IPF) 对 航 迹 进行 跟踪 的 比较 示例 ， 
图 8.3 为 100 次 独立 实验 的 目标 航 迹 MSE 比较 ， 两 图 中 N=1000. 

由 图 8.2 中 可 以 看 出 ， 基 于 粒子 滤波 的 跟踪 性 能 比 EKF 和 UKF 有 了 较 大 的 改善 ， 
这 不 仅 体 现在 对 状态 的 估计 精度 上 ， 同 时 也 体现 在 对 目标 的 跟踪 能 力 上 。 由 图 8.3 可 以 
清楚 地 看 出 ， 在 成 功 跟 踪 的 前 提 下 ，EKF 和 UKF 的 跟踪 误差 基本 上 在 同一 量 级 ， 但 这 
并 不 意味 着 两 者 性 能 相近 。 应 当 看 到 ， 在 100 次 实验 中 EKF 有 26 次 MSE 超过 1， 这 说 
明 ， 这 些 跟 踪 产 生 了 严重 的 发 散 。 相 对 而 言 ，UKF 要 好 得 多 ， 图 8.3 中 所 示 MSE 超过 1 
的 仅 有 1 次 。 
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图 8.2 目标 航 迹 及 其 估计 
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图 8.3 100 次 独立 试验 MSE 比较 


很 明显 ， 基 于 粒子 滤波 的 跟踪 算法 都 没 出 现 明显 的 跟踪 发 散 现象 ， 其 估计 误差 也 要 
小 得 多 。 为 了 更 好 地 比较 SPF 和 IPF 两 种 粒子 滤波 器 的 性 能 ， 将 不 同 条 件 下 的 仿真 结果 
列 于 表 8.1 中 。 需 要 说 明 的 是 ， 由 于 EKF 和 UKF 两 种 方法 都 有 发 散 现象 ， 对 MSE 作 简 
单 的 平均 比较 并 无 实际 意义 ， 因 此 在 表 中 并 未 列 出 。 


表 8.1 不 同 粒子 数目 条 件 下 的 仿真 结果 比较 


0.005013 0.004409 
0.003779 0.003348 
0.003582 0.002833 


0.003203 0.002677 
0.002460 0.001948 
0.002161 0.001864 
0.002027 0.001713 
0.001996 0.001605 





图 8.4 对 两 种 算法 在 Matlab 仿真 环境 下 的 平均 运行 时 间作 了 比较 。 图 中 给 出 的 是 程 
序 实际 运行 时 间 对 SPF 算法 在 N=500 条 件 下 运行 时 间 的 归 一 化 结果 。 从 图 中 的 结果 看 ， 
在 粒子 数 相 同 的 前 提 下 ，IPF 的 运行 时 间 比 SPF 有 所 增加 ， 前 者 约 为 后 者 的 2.2 倍 ( 若 
对 IPF 算法 进行 某 种 优化 ， 可 进一步 降低 相对 运行 时 间 )。 对 比 表 8.1 的 结果 可 以 发 现 ， 
在 运算 时 间 开 销 相 同 的 条 件 下 ，IPF 的 估计 结果 仍 要 优 于 SPF. 

总 之 ，IPF 算法 通过 对 最 优 采样 函数 的 合理 近似 ， 能 产生 更 有 效 的 粒子 来 逼近 真实 
的 状态 分 布 ， 同 时 ， 通 过 MCMC 策略 来 减轻 重 采样 可 能 导致 的 粒子 贫 化 现象 ， 从 而 提 
高 算法 的 估计 性 能 。 对 只 测 角 跟 踪 的 仿真 结果 表明 ，IPF 算法 比 EKF 和 UKF 具有 更 高 
的 精度 和 更 强 的 鲁 棒 性 ; 与 SPF 相 比 ， 其 性 能 也 有 较 大 的 提高 ， 并 可 以 在 相同 的 估计 精 
度 要 求 下 大 大 减少 所 需 的 粒子 数目 。 
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Relative Time Consumptio 





500 1000 1500 2000 
Number of Particles 


图 8.4 两 种 算法 时 间 复杂 度 比 较 


8.4.2 MUPF 的 被 动 跟踪 

1， 只 测 角 跟踪 

仍 以 8.4.1 节 中 的 只 测 角 跟 踪 为 例 ， 验 证 跟踪 算法 的 性 能 。 为 区 别 起 见 ， 这 里 将 采用 
更 一 般 的 过 程 噪声 形式 。 目 标的 状态 方程 为 


x, = Fx, , + Iw, 


I, T.I 0.57? -I 
x, 2D, 5 3; A4] » w, = [w, w,]; r-e i r-| i 
k k Jk =" Jk k y 0 £ T-E, 


过 程 噪声 为 w ~ NN(0,Q) ，Q = 0.001 .7 了; 观测 方程 及 其 他 仿真 参数 设置 与 前 一 实 
例 保持 一 致 。 在 不 同 的 粒子 数目 条 件 下 ， 分 别 用 UPF 和 MUPF 对 目标 的 状态 进行 估计 。 
算法 性 能 用 MSE 和 平均 有 效 采 样 数 (Averaged Effective Sample Number, AESN) 来 评 
估 ，AESN 定义 为 


(8-58) 
式 中 


1 K N ~(i)\-2 
AESN = 2 ua Din (a) (8-59) 


AFP: 天 为 每 次 仿真 的 观测 次 数 ;， N 为 粒子 数 。 

注意 到 协 方差 矩阵 ror" 的 奇异 性 ， 先 验 p(x | x ) 不 能 通过 式 (8-34) 直接 计算 
得 到 ， 故 在 传统 UPF 算法 中 必须 采取 特殊 的 措施 来 处 理 这 一 问题 。 一 种 可 能 的 方法 是 将 
高 维 状态 空间 问题 转化 为 低 维 噪 声 空 间 问 题 来 处 理 。 由 式 〈8-$8) 可 得 

p(x, | x, ,) = p,,(G(x, — Fx,_,)) (8-602 

AP: G-(l Ty) 大。 经 过 这 种 转化 ， 就 可 以 用 相对 简单 的 方法 计算 粒子 的 等 效 权重 。 

8.5 为 两 种 滤波 器 对 目标 进行 单 次 跟踪 的 示例 ; 图 8.6 为 100 次 独立 实验 中 目标 位 
置 估计 的 MSE， 粒 子 个 数 为 200; 图 8.7 为 100 次 仿真 中 AESN 的 变化 情况 ， 图 8.8 比 
较 了 不 同 粒子 数目 条 件 下 ， 两 种 算法 MSE 的 均值 和 标准 偏差 。 

如 图 8.5— FH 8.8 所 示 ， 在 相同 的 仿真 条 件 下 ，MUPF 要 明显 优 于 UPF。 究 其 原因 ， 
主要 是 因为 MUPF 算法 能 有 效 改 善 粒 子 的 权 值 分 布 特性 ， 使 粒子 的 多 样 性 得 以 有 效 保持 


136 


(这 从 图 8.7 可 以 清楚 地 看 出 )， 从 而 有 效 提高 滤波 器 的 性 能 。 由 图 8.8 可 知 ，MUPF 算法 
的 MSE 均值 和 标准 偏差 都 较 UPF 要 小 ， 这 说 明 MUPF 不 仅 有 更 优 的 估计 精度 ， 算 法 的 
稳定 性 也 得 以 提高 ， 收 敛 一 致 性 也 更 好 。 





-2 
-0.06 -0.05 -0.04 -0.03 -0.02 -0.01 0 0.01 002 
axis-x 


图 8.5 目标 真实 位 置 与 位 置 估计 ( N=200 ) 


0? 
25r 


MSE for Trajectory Estimation 





图 8.6 100 次 独立 实验 位 置 MSE ( N=200 ) 


Averaged Effective Sample Number 





图 8.7 100 次 仿真 中 平均 有 效 粒 子 数 
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Mean and Standard Deviation of MSEs 
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图 8.8 目标 位 置 MSE 均值 和 标准 偏差 

2.， 方位 一 频率 联合 跟踪 

假定 目标 初始 位 置 和 速度 分 别 为 (50, 80) km 和 (-150,50)m/s ， 过 程 噪声 协 方差 矩阵 
Q, = diag((2m/s2)?,(2m/s2)2) 。 进 一 步 假 定 T=ls , f,-3GHz (多 普 勒 频率 观测 模型 见 9.3.1 
节 )， 观 测 精度 为 {3mrad，10Hz}。 采 用 UPF 和 MUPF 分 别 对 其 跟踪 ， 粒 子 数目 N=400， 
仿真 次 数 为 100， 所 得 结果 如 图 8.9 和 图 8.10 所 示 。 

由 图 8.9 和 图 8.10 可 以 看 出 ,两 种 滤波 器 都 能 以 较 快 的 速度 收敛 。 但 相 比 之 下 , MUPF 
无 论 在 收敛 速度 还 是 跟踪 精度 方面 都 要 优 于 UPF， 表 明 前 者 能 得 到 更 准确 的 状态 估计 。 
本 例 中 的 仿真 结果 与 只 测 角 跟踪 的 结论 是 完全 一 致 的 ， 这 就 说 明 ，MUPF 在 稳定 性 、 跟 
踪 速 度 和 精度 等 方面 确实 比 UPF 有 了 明显 的 改善 ， 是 一 种 更 为 有 效 的 跟踪 算法 。 

综合 两 个 仿真 实例 的 结果 可 知 ，MUPF 算法 能 有 效 解 决 传统 UPF 中 的 潜在 不 足 , 通 
过 对 权 值 的 修正 可 以 明显 改善 其 分 布 特性 ， 从 而 使 粒子 的 多 样 性 得 以 保持 。 概 言 之 ， 
MUPF 与 传统 UPF 算法 相 比 具有 两 大 潜在 优势 : 一 是 在 同样 的 估计 性 能 要 求 下 ， 可 以 大 
大 减 小 粒子 数目 ， 从 而 有 效 降低 运算 开销 ; 另 一 个 优点 在 于 当 过 程 噪 声 协 方差 矩阵 奇异 
时 ， 对 重要 权 的 计算 无 需 采 取 任 何 特殊 措施 ， 使 得 MUPF 算法 更 便于 实现 。 此 外 ， 从 整 
个 算法 流程 可 以 看 出 ， 两 种 算法 具有 相同 的 复杂 度 ， 这 一 复杂 度 约 为 UKF 的 入 倍 (N 
为 粒子 数 )。 


— True 
--- UPF j 
--e-- MU PF £/ 


# 


axis-y(m) 





3 35 4 4.5 5 5.5 6 65 7 
axis-x(m) x]10* 


图 89 两 种 算法 跟踪 结果 比较 
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图 8.10 ”相对 距离 误差 比较 
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本 章 主要 阐述 基于 序 贯 重要 采样 的 粒子 滤波 原理 及 其 在 目标 跟踪 中 的 应 用 问题 。 粒 
子 滤波 的 关键 是 如 何在 给 定 算 法 复杂 度 的 条 件 下 得 到 真实 后 验 状 态 分 布 的 有 效 近似 ， 传 
统 粒子 滤波 器 直接 根据 状态 转移 方程 产生 粒子 ， 由 于 没有 考虑 量 测 的 修正 影响 ， 估 计 性 
能 较 差 。 针 对 这 一 问题 ， 提 出 了 一 种 通过 最 优 采样 函数 近似 生成 粒子 的 方法 ， 并 加 入 
MCMC 过 程 来 保持 粒子 的 多 样 性 ， 从 而 提高 了 滤波 器 的 估计 性 能 。 

UPF 算法 通过 UKF 的 滤波 结果 产生 建议 分 布 , 由 于 UKF 具有 良好 的 性 能 ，UPF 被 认 
为 是 一 种 较为 有 效 的 滤波 算法 。 但 在 被 动 跟踪 背景 下 ， 跟 踪 误差 与 噪声 方差 相差 悬殊 ， 容 
易 出 现 数值 敏感 问题 ， 导 致 算法 性 能 严重 下 降 。 为 此 文中 提出 一 种 修正 算法 一 一 MUPF， 
通过 对 权 值 的 修正 使 其 避免 数值 问题 ， 并 有 效 减轻 了 粒子 贫 化 现象 ， 从 而 提高 滤波 效果 。 

值得 一 提 的 是 ， 理 论 上 ， 本 书 第 二 部 分 阐述 的 其 他 形式 的 高 斯 滤波 算法 都 可 用 做 指 
导 粒 子 生 成 的 建议 分 布 ， 由 此 得 到 各 种 基于 重要 性 采样 函数 的 粒子 滤波 实现 形式 ， 由 于 
其 原理 相似 ， 在 此 不 再 著述 。 另 外 ， 除 了 前 文 所 述 的 几 种 建议 分 布 函 数 形式 外 ， 目 前 还 
存在 一 些 其 他 形式 ， 感 兴趣 的 读者 可 参阅 文献 [41]。 
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第 9 章 边缘 化 采样 滤波 


尽管 从 前 面 的 仿真 结果 可 以 发 现 ,粒子 滤波 算法 比 传统 滤波 算法 如 EKF. UKF 等 有 
着 更 广泛 的 适用 性 和 更 强 的 跟踪 能 力 ， 但 同时 也 清楚 地 看 到 ， 这 些 优 势 的 获得 是 以 增加 
运算 量 为 代价 的 。 理 论 上 ， 在 同等 近似 精度 要 求 下 ， 粒 子 滤 波 算法 中 所 需 的 粒子 数 与 状 
态 量 的 维 数 呈 指数 增长 ， 这 一 运算 量 对 许多 高 维 状态 空间 的 估计 问题 是 难以 承受 的 。 

但 在 很 多 特殊 的 情况 下 (包括 目标 跟踪 应 用 场合 )， 系 统 包含 一 个 线性 的 子 结构 ， 即 
系统 模型 中 部 分 状态 可 用 线性 模型 来 描述 。 这 就 意味 着 ， 系 统 的 线性 部 分 可 以 用 最 优 卡 
尔 曼 滤波 进行 估计 ， 而 粒子 只 需 存在 于 低 维 的 非 线 性 状态 空间 ， 从 而 在 给 定 粒 子 数目 的 
条 件 下 使 估计 性 能 得 以 改善 。 这 种 从 总 的 系统 状态 PDF 中 提取 线性 状态 分 布 的 方法 称 为 
“边缘 化 ” 相应 的 估计 算法 称 为 边缘 化 粒子 滤波 (Marginalized Particle Filtering) ^9! 
或 Rao-Blackwellized FIE 6, 

本 章 首先 介绍 了 混合 系统 状态 〈 这 里 特 指 线性 与 非 线性 部 分 的 混合 ) 通用 的 边缘 化 
粒子 滤波 处 理 思路 ， 并 给 出 了 相应 的 算法 流程 ， 在 此 基础 上 重点 针对 几 种 重要 的 系统 模 
型 (如 对 角 模型 、 三 角 模 型 及 一 类 与 目标 跟踪 密切 相关 的 特殊 模型 )， 详 细 阐 述 了 其 边缘 
化 滤波 方法 ， 并 分 析 了 算法 的 复杂 度 ， 最 后 给 出 了 算法 的 目标 跟踪 应 用 实例 。 


9.1 混合 状态 的 边缘 化 


如 前 所 述 ， 粒 子 滤波 方法 可 以 递 推 地 产生 随机 采样 点 来 近似 后 验 PDF p(x, | yu)» 
为 非 线 性 、 非 高 斯 系统 中 的 状态 估计 问题 建立 了 统一 的 处 理 框 架 。 但 由 于 众所周知 的 原 
因 ， 当 状态 量 的 维 数 比 较 高 时 ， 必 须 产生 大 量 的 粒子 才能 较 好 地 近似 高 维 状态 的 PDF, 
导致 PF 在 许多 实际 情况 中 并 不 适用 。 

然而 ， 在 很 多 特定 情况 下 ， 系 统 模型 都 具有 可 解析 求解 的 子 结构 ， 因 此 在 非 线性 状 
态 量 已 知 的 条 件 下 ， 可 以 将 线性 状态 “边缘 化 ”出 来 ， 使 PF 只 工作 于 较 低 维 的 非 线 性 
状态 空间 。 其 基本 思路 是 将 状态 变量 分 成 两 部 分 ， 即 

x, =[(xz) (T (9-1) 
AP: xx 为 依赖 于 系统 条 件 的 线性 状态 量 ; x, 为 非 线 性 状态 量 。 若 令 xo, (DNE, MM 
由 贝 叶 斯 定理 易 得 
PG, Xo | Vix) = p(Xk | Xox ix) P(xXor | ia) (9-2) 
式 中 : p(x, | xt, Yu) 易于 解析 求解 ,可 通过 最 优 卡 尔 曼 滤波 器 (KF) 来 实现 ; 而 p(x7, | y,,) 
则 无 法 求 得 解析 解 ， 可 通过 PF 来 实现 。 
下 面 将 给 出 边缘 化 滤波 的 一 般 处 理 流程 ， 总 结 如 下 。 


Alg (9.1): Generic Marginalized Particle Filtering 
步骤 1: 初始 化 

For i=1,2,:::,N 
对 粒子 xy) -p(xa) 和 集合 (x PO = Gu P.) 进行 初始 化 。 
步骤 2: 粒子 量 测 更 新 

For i=1,2,::. N 
WPRCTAKUOE wP =p(y, | xox) Via) 并 进行 归 一 化 : 

ü= Y uf 

j=1 


步骤 3: 粒子 重 采 样 
对 X 个 粒子 进行 重 采样 使 得 Pr( xp? 5x5 )-up . 
步骤 4: 粒子 滤波 时 间 更 新 及 卡尔 曼 滤波 
(OD 卡尔 曼 量 测 更 新 。 
(2) 粒子 滤波 时 间 更 新 。 
For i=1,2,::. N 
预测 新 粒子 其 全 ^pa | X ha) o 
(3) 卡尔 曼 时 间 更 新 。 
ZR: k=k+1， 从 步骤 2 开始 进行 下 一 轮 递 推 


92 ”边缘 化 粒 了 于 滤波 算法 
9.2.1 ”对 角 模 型 算法 


考虑 下 式 所 示 的 系统 模型 
x, =f (x, i) tw, 
= " A, 4 (x; 5. +w, 


yhp C, pL +, 
Xm: W. 为 过 程 噪 声 ， 且 


1 l 
"| vag). o|% M 
k k 


(9-3) 


(9-4) 


v, AEWRE H v, ~N (0,R,) 5. x R) 394378 Hx, (KP): xo 的 分 布 可 任意 但 必 


须 已 知 ，A 、C 为 任意 矩阵 。 


HFA (9-3) 表示 的 模型 具有 类 似 于 对 角 结 构 的 特点 , 因此 该 模型 可 称 为 对 角 模 型 。 
贝 叶 斯 滤波 估计 的 目的 是 利用 粒子 滤波 算法 得 到 后 验 密度 p(xi |y;)， 但 是 由 式 (9-3) 


"[Al, fEx, 已 知 的 条 件 下 ， 该 式 存 在 一 个 线性 的 子 结构 。 根 据 式 (9-2) 表示 的 边缘 化 
公式 ， p(xk | xo, Ji) 可 通过 卡尔 曼 滤波 解析 求解 ， 而 P(xtx | yu.) 则 可 通过 粒子 滤波 求 
解 。 可 以 预见 ， 在 粒子 数 相同 的 条 件 下 ， 采 用 边缘 化 粒子 滤波 得 到 的 估计 比 传统 的 粒子 
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滤波 估计 效果 要 好 , 这 主要 归结 为 两 方面 的 原因 : 一 是 P(xt | yia) 的 维 数 比 p(xi xr | yu) 


要 低 ， 表 明 粒 子 处 于 较 低 维 的 空间 ， 因 而 近似 精度 更 高 ， 二 是 在 估计 线性 状态 量 时 采用 
了 最 优 算法 ， 相 应 地 性 能 也 将 会 更 佳 。 对 于 任意 非 线 性 函数 g(-)， 若 定义 


I(gG,))-E, y, (g())= [Gp n, )dx, (9-5) 
H £5 (e) M Êe (05) 分 别 为 利用 N 个 粒子 得 到 的 对 式 〈9-5) 的 标准 粒子 滤波 和 
边缘 化 粒子 滤波 估计 ， 则 当 N — oo 时 ， 根 据 中 心 极限 定理 有 下 列 结论 成 立 ， 即 
JN[Íy (gGx,)) - 1(g(x,))]> M (0,02) (9-6) 
VNI? (gG%,)) - 1(g(x,))]=> A (0,02) (9-7) 
RH: o > oa2 。 关 于 该 式 的 详细 证 明 见 文献 [7]。 
下 面 详细 分 析 p(x, | x Va) 与 pO, | yu) 的 求解 过 程 。 事 实 上 ， 对 于 第 一 项 ， 由 
卡尔 曼 滤波 公式 ， 易 得 x! 和 ,i 的 条 件 概率 密度 函数 为 


p(x | X545 ViN (x, 9 (9-8) 
Pa | Xo4415J14) V. bh Ls Peak) (9-9) 
式 中 
Ld Xi +K, (y, -h — Cei) (9-10) 
P, =P- - KC, Pp (9-11) 
K,7P,, Cr S, (9-12) 
S, =C,P -Ck +R, (9-13) 
1 7A, XL. (9-14) 
Py. A, Pa (4a) +Q, (9-15) 


上 述 递 推 式 的 初始 条 件 为 刀 =S, P, =P, 
对 于 第 二 项 p(xo, | yx)， 根 据 贝 叶 斯 理论 ， 有 

je: P4» Jia) » DX,» Jy Xo A Jii) 

PO) PGOX | Vix) Pik) 

= BG Y. | Xox Yi) 

PGY | Yu) 
_ PO Kox Jua JPG, | Xor- Vix) 
PO. Da) 

XP: Poralia) 可 由 前 一 时 刻 (大 -1 时 刻 ) 的 粒子 滤波 估计 得 到 ;解析 表达 式 

PO, Ioa iai) 及 p(Xk | Xor-1 Vir) 可 表示 为 


p(xox | Yi 


p(xXox-1 | Vix) (9-16) 


Pa lia) 


PO lu) Jia EN (f tC,xi,,C, P, Ck *R,) (9-17) 
PG, |X0 u] 44) N G5 fia) (9-18) 


由 此 可 见 ， 式 〈9-3) 的 状态 估计 可 通过 粒子 滤波 (xz 0 y 和 卡尔 曼 滤 波 得 到 ， 对 于 
每 一 个 粒子 ， 需 要 一 个 相应 的 卡尔 曼 滤 波 器 。 完 整 的 滤波 算法 流程 可 见 Alg (9.1)， 由 该 
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算法 中 可 以 看 出 ， 与 标准 的 粒子 滤波 相 比 ，MPF 算法 在 时 间 更 新 〈 预 测 ) 阶段 发 生 了 变 
化 。 在 SPF 算法 中 ， 预 测 过 程 仅 由 Alg (9.1) 中 的 步骤 4 (2) 来 完成 ， 步 骤 4 (1) 在 
卡尔 曼 滤 波 中 称 为 量 测 更 新 。 另 外 ， 非 线性 状态 量 的 预测 xiu, 是 通过 步骤 4 (2) 得 到 
的 。 根 据 式 (9-3) 中 第 一 式 可 知 ， 非 线性 状态 量 的 预测 不 包含 线性 状态 的 任何 信息 ， 表 
明 xp, 不 能 用 于 提高 线性 状态 量 的 估计 质量 。 但 是 ， 若 在 式 〈9%-3) 第 一 式 中 线性 状态 


与 非 线性 状态 之 间 具 有 某 种 依赖 关系 ， 则 非 线性 状态 可 用 于 提高 线性 状态 的 估计 效果 ， 
这 一 点 将 在 9.2.2 节 中 进行 分 析 。 
9.2.2 ”三角 模型 算法 
KHR (9-3) 进行 扩展 并 包含 如 (xz)x! 项 ， 即 可 得 到 如 下 三 角 模 型 : 
š = f Gt) AL Gg.) tw 
x, = AL ka tw) (9-19) 
y, = h(x,)* C, (x, x, tv, 
H (9-19) 第 一 式 可 以 看 出 ， 癌 ,1 包含 了 线性 状态 的 信息 ， 这 意味 着 在 非 线性 状 
态 的 预测 xp, 中 含有 线性 状态 x(_， 。 为 了 说 明 这 一 点 ， 假 定 在 Alg (9.1) 的 步骤 4 (2) 
中 已 经 处 理 完成 ， 即 r 已 经 是 可 用 的 ， 此 时 该 模型 可 写成 
"a Xa va (9-20) 
Zk — 2X T WEA 
AP: gax 7/05 TRARRE wr. 的 量 测 。 
由 于 式 〈9-20) 为 线性 状态 空间 模型 ， 含 有 高 斯 分 布 噪声 ， 其 最 优 状态 估计 可 由 卡 
尔 曼 滤波 得 到 且 
Sr =% HL, G, — Ar.) 
P; =P, - L,N,L; 
L, =P, APY NE 
N,=4P P (4P) +Q; 
ARP: 上 标 “*” 用 于 与 第 一 次 量 测 更 新 进行 区 分 。 总 和 P, 可 通过 与 式 (9-10) 和 式 (9-11) 
相 类 似 的 方法 得 到 。 
最 后 一 步 是 将 第 二 次 量 测 更 新 与 时 间 更 新 进行 合并 得 到 预测 状态 ， 其 结果 可 表示 为 
fin =A; 3 +L, G, — A; 5 ) 
Pay -A,P, (A, y +Q, -L,N, L, 
L,-A,P,(Ap)) N; 
N,-Aj P, (AP) +Q; 
式 (9-22) 的 详细 证 明 见 文献 [42], 为 了 使 Alg (9.1) 中 的 算法 适用 于 该 模型 , zÇ (9-14) 
FI (9-15) 中 的 时 间 更 新 方程 必须 用 式 (9-22) 代替 。 


(9-21) 


(9-22) 
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923 ”特殊 模型 算法 


在 实际 应 用 中 ， 存 在 一 类 重要 的 系统 ， 具 有 线性 状态 方程 和 非 线性 量 测 方程 ， 可 用 
如 下 模型 表示 为 


xi = Aí xr 十 AL xi +w (9.23) 
Xin 7 Box, + Bux, +W, 
yy = h(xj) + v, (9-24) 


AP: wN OQ): w,-N (0,0) :. v, AENDA RERE, 

上 述 模型 广泛 应 用 于 描述 直角 坐标 系 中 的 目标 运动 及 各 种 量 测 坐 标 系 中 的 系统 观 
测 ， 在 目标 跟踪 等 领域 具有 重要 的 用 途 。 在 该 模型 中 ， 式 (9240 中 未 包含 任何 关于 线 
性 状态 x 的 信息 ， 因 此 就 卡尔 曼 滤 波 而 言 ， 式 (9-240 不 能 用 于 估计 线性 状态 ， 相 反 ， 
所 有 的 观测 信息 通过 第 二 次 量 测 更 新 隐 含 地 融入 到 卡尔 曼 滤波 中 ， 这 意味 着 Alg (9.1) 
中 的 步骤 4 (1) 步骤 可 以 省 略 。 在 这 种 条 件 下 ， 第 二 次 量 测 更 新 不 仅仅 只 是 第 一 次 量 测 
更 新 的 修正 ， 而 是 观测 信息 y 融入 算法 中 的 唯一 途径 。 


鉴于 该 模型 的 重要 性 ， 下 面 给 出 基于 该 模型 的 完整 的 滤波 算法 流程 。 
Alg (9.2): Marginalized Particle Filtering (MPF) 
步骤 1: 对 N 个 粒子 进行 初始 化 ， 使 得 xio ~ p(x?)(i=1,2,…,N) B (x10, pe) y) = 
(xo, A) ° 
步骤 2: 计算 权重 wk) = p(y, xo Vir) Py (y, - h(x)) JEIH —1648 51 GP 。 
步骤 3: HATER, EE Pr(x; ? = xr) = up. 
步骤 4: 粒子 时 间 更 新 和 卡尔 曼 滤 波 更 新 。 
卡尔 曼 量 测 更 新 ， 即 
g" = i , P, = Pi 
© 粒子 时 间 更 新 (预测 )， 即 
x ~ pa P a) 
=N (xt iA xr O HASO, OF + AP, CAL) ) 
© 卡尔 曼 滤波 时 间 更 新 ， 即 
$15 = BL OBL SUR, G25) -AEO - A 319) 
Pn B, P, (B, y +Q, -K,M,K, 
M,-A,P,(4,)' +Q; 
K,-B,P,(4,) M; 
步骤 5: 重复 步骤 (2) 一 步骤 (4)。 
MPF 算法 的 复杂 度 取决 于 实际 的 模型 和 边缘 化 手段 ， 关 于 这 一 点 文献 [47] 已 有 详细 
的 研究 和 分 析 ， 对 于 式 〈9-23) 和 式 〈9-24) 所 示 的 模型 ， 若 非 线性 状态 和 线性 状态 的 
维 数 分 别 为 x 和 7， 粒子 个 数 为 N， 则 算法 复杂 度 为 
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C(n,l, Ny-AnP *8ln? +4n°/3+5P — Sn1+27? 
H6nl+4n? +21 +n — l+nc, +e +c,)N 
AF: c 为 计算 高 斯 似 然 的 复杂 度 ; c, 为 重 采 样 的 复杂 度 ， c, 为 产生 随机 数 的 复杂 度 。 
对 于 工 维 状态 量 x ， 根 据 文献 [47] 的 分 析 ， 在 相同 运算 复杂 度 条 件 下 《〈 即 
C(nt+1,0,Nope)=C(n,1,NMps) ) MPF 算法 与 SPF 算法 所 具有 的 粒子 个 数 之 比如 图 9.1 所 示 。 


| C(nH, 0, Nees) =C(n, 1, Nupp) 





Number of states in the Kalman filter(/) 


图 9.1 相同 运算 开销 条 件 下 MPF 与 SPF 粒子 个 数 比 


93 应 用 实例 


9.3.1 被 动 雷达 目标 跟踪 
考虑 二 维 平面 内 的 被 动 目标 定位 与 跟踪 问题 ， 目 标 运 动 可 描述 为 


X, n = F,x, + W, (9-25) 


Rs x= Ds n AIRES z, =| 0 r ankata, raman 
2 


间隔 ，w 为 过 程 噪声 且 w, ~ N (0,0) 。 
观测 量 包 括 方位 角 〈w: )， 角 度 变 化 率 ( gp, ) 和 多 普 勒 频率 〈 f )， 所 得 非 线性 观 
测 方程 为 
arctan ( x, | y,) 
y, - hou) ev, 2| 28799. (uy, (9-26) 
P 
fite) | L 
AU: n = x: * yr: c 为 光速 f, 为 辐射 源 频率 。 
显然 ， 上 述 模 型 中 包含 了 关于 目标 速度 的 线性 子 结构 。 若 定义 x? =[x 六 ， 
x! =[x%, n] > WR (9-25) AR (9-260 所 表示 的 模型 可 重新 记 为 [9 
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n Qn l n 
Xk = Xk + Tx, + Wii 


š i I (9-27) 
x, = Xk-1 TW 
° | m / y, | | 0 0 | 
y, 7| ë |= 0 +| n -x Ir |x +v (9-28) 
L hn — fox Kern) -foyr Ker) 
ma — € o 0 


式 中 
w;- N(0,Q;): w,-N(0,Q)) » v, — N(Q0,R,) 

本 实例 中 , 首先 通过 仿真 验证 MPF 算法 的 有 效 性 , 随后 将 其 应 用 于 被 动 目标 定位 的 
外 场 试 验 数据 滤波 处 理 ， 同 时 对 SPF 和 MPF 的 性 能 进行 比较 。 

仿真 中 ， 假 定 观测 器 作 折 线 运动 ， 目 标 初始 位 置 和 速度 分 别 为 xo,m) = (30,50) km 
和 (3,59) 2 (7150,50) m/s， 两 者 航 迹 如 图 92 所 示 。 进 一 步 假定 T-1s, f,-3GHz, 
Or =Q! =41,, P,-diag(le4,le4) 。 方 位 角 、 角 度 变 化 率 和 多 普 勒 频率 的 测量 精度 分 别 
为 Smrad，0.3mrad/s 和 15Hz。 仿 真 中 粒子 数目 N=500， 蒙 特 卡 罗 次 数 为 100， 所 得 结果 
如 图 9.3 所 示 。 


axis-y(km) 





0 5 10 15 20 25 30 
axis-x(km) 


H92 目标 与 观测 器 航 迹 


Relative Range Error(%) 





图 9.3 仿真 场景 中 的 相对 距离 误差 
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由 图 9.3 可 以 看 出 ， 在 开始 一 段 时 间 内 ，SPF 和 MPF 的 收敛 速度 都 较 快 ， 其 相对 距 
离 误 差 在 15s 之 内 由 开始 的 4$% 下 降 到 10% 左 右 。 相 比 之 下 ，MPF 比 SPF 更 加 有 效 ， 这 
主要 体现 在 收敛 时 间 和 跟踪 误差 两 方面 。 究 其 原因 , 是 由 于 在 MPF 中 粒子 处 于 更 低 维 的 
状态 空间 ， 从 而 在 给 定 算 法 复杂 度 的 条 件 下 能 产生 更 多 有 效 的 粒子 来 近似 真实 的 PDF， 
提高 估计 性 能 。 

试验 场景 与 第 4 章 中 的 图 4.4 相同 ， 观 测 器 到 目标 航 迹 的 垂直 距离 约 S20m， 辐 射 源 
的 运动 速度 约 为 15m/s。 所 不 同 的 是 ， 这 里 测量 参数 为 方位 角 、 角 度 变 化 率 和 多 普 勒 频 
率 ， 其 测量 精度 分 别 为 0.36mrad, 2.5mrad/s 和 10Hz。 试 验 中 T=0.5s ，N=400 ， 分 别 用 
SPF 和 MPF 对 目标 进行 跟踪 ， 其 结果 如 图 9.4 和 图 9.5 所 示 。 


axis-y(m) 





-400 -200 0 200 400 600 800 
axis-x(m) 


图 9.4 试验 场景 中 的 目标 航 迹 跟踪 


Relative Range Error(%) 





Time(s) 


图 9.5 试验 场景 中 的 相对 距离 误差 


从 图 9.4 和 图 9.5 可 以 清楚 地 看 到 ，MPF 在 外 场 试验 数据 处 理 中 取得 良好 的 效果 ; 同 
时 可 以 看 出 ，MPF 的 跟踪 性 能 比 SPF 更 好 ， 这 一 点 与 前 面 的 理论 分 析 和 仿真 结果 是 完全 
一 致 的 。 与 数值 仿真 相 比 ， 试 验 中 的 跟踪 误差 更 小 ， 这 是 由 观测 器 和 目标 的 相对 位 置 和 距 
离 决定 的 ， 在 其 他 条 件 相同 的 情况 下， 两 者 之 间 的 距离 越 小 ， 跟 踪 精 度 越 高 ， 反 之 亦 然 。 

仿真 和 试验 结果 均 表 明 ，MPF 在 收敛 速度 方面 要 优 于 传统 SPF， 是 一 种 较为 有 效 的 
估计 算法 ， 同 时 该 算法 可 以 有 效 降低 估计 误差 ， 改 善 跟踪 性 能 ， 因 此 在 被 动 目标 跟踪 应 
用 中 具有 潜在 的 优势 。 
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9.3.2 ”红外 检测 前 跟踪 
考虑 二 维 平面 内 的 目标 检测 前 跟踪 问题 ， 目 标的 运动 状态 满足 
x,, = Fx, + W, (9-29) 
AP: x,=[x, X, y, XL] 为 状态 矢量 ; F=“4dia([F,, F1) 为 状态 转移 矩阵 ， 


r- 中 7 为 观测 时 间 间 隔 ; w, 为 过 程 噪声 且 w, ~ N(0,O,) 。 


目标 观测 方程 为 
"m i dod (9-30) 
vi H, 
. 2 . 2 
xm. i^e) e L i I, MEER, AFAA 
Tt 
T; vD 为 第 心力 个 分 辨 单元 的 观测 噪声 ; H A H, RREME S BUJffrfEPEHERYW, H, 
示 有 目标 ，HH, 表示 无 目标 。 
针对 上 述 系 统 模型 结构 ， 若 定义 xf =[x yy L], x, =[Xx y] » WR (9-290 Br 
示 的 模型 可 重新 描述 为 


» n l n 
x, = Ax; + Bx, + Wia 


l I I (9-31) 
X, = Cx, + Wh 
AP: 
100 T 0 Pa 
we~ N,Q), wi~ N09), 4-0 1 o|, B-|0 T), c- | 
0 0 1 0 0 
此 时 ， 观 测 方程 可 进一步 记 为 
(ij) ( =n (iJ) 
z®) = i et Vk H, (9-32) 
w” H, 


AP: ov^? — N(0,o2); er 为 噪声 功率 。 
由 此 可 见 ， 上 述 模型 满足 9.2.3 节 中 介绍 的 系统 模型 形式 , 可 采用 边缘 化 粒子 滤波 算 
法 进行 处 理 。 不 过 ， 与 单纯 的 目标 跟踪 不 同 ， 对 于 检测 前 跟踪 问题 ， 不 仅 要 实现 对 目标 
运动 状态 的 估计 ， 同 时 还 要 对 目标 的 存在 性 进行 判断 ( 即 进行 目标 检测 )。 为 此 ， 通常 需 
要 引入 一 独立 变量 E, e (0,1) 来 表示 目标 的 出 现 与 否 ， 其 中 0 表示 目标 未 出 现 , 而 1 表示 
目标 出 现 。 
若 定义 目标 “出 现 ” 概 率 愉 和 “消失 ”概率 己 分 别 为 
P, 5 P(E, -1| E, , =0) (9-33) 
P, 5 PLE, «0| E, , 51) (9-34) 
同时 假定 目标 持续 出 现 概 率 〈 存 活 概率 P{E, 21| E, =) 和 目标 持续 不 出 现 概率 
P(E, -0|E, ,= 全 分 别 为 1- 已 和 1- 已 ， 则 可 得 到 关于 目标 存在 性 的 转移 概率 矩阵 为 


mg- l^ B (9-35) 
P, 1-B, 
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在 具体 应 用 中 ， 一 般 假设 目标 在 初始 时 刻 的 出 现 概 率 PE, =1) 已 知 ， 用 于 表征 目标 
出 现 与 否 的 变量 6 通常 可 利用 以 下 两 种 途径 得 到 。 

第 一 种 为 混合 状态 估计 ,在 目标 状态 向 量 中 增加 该 离散 变量 , 实现 目标 状态 与 5 的 
混合 估计 过 程 , 即 估 计 和 密度 p, E, =1|zx) 。k 时刻 的 独立 变量 基于 转移 概率 矩阵 并 
和 kl 时刻 的 独立 变量 E... 计算 得 到 ， 目 标 状态 x, 由 粒子 滤波 处 理 获取 。 完 成 滤波 后 ， 
计算 x 时刻 表示 目标 出 现 〈( E =1) 的 粒子 数量 ( N.) 与 粒子 总 数 (N) 的 比值 ， 将 该 
比值 与 预定 义 门限 ( PB ) 进行 比较 ， 超 过 门限 即 认 为 检测 到 目标 ， 反 之 ， 则 判定 目标 没 
有 出 现 。 

第 二 种 算法 仍然 是 估计 密度 pæ, E, =1|z)， 但 是 处 理 中 将 独立 变量 的 估计 从 
目标 状态 向 量 中 分 离 出 来 ， 采 取 更 为 有 效 的 出 现 概率 计算 方法 ， 具 体 处 理 过 程 这 里 不 再 
详 述 ， 可 参见 文献 [49] 和 文献 [50]。 分 离 的 结果 使 得 对 于 粒子 的 使 用 更 为 有 效 ， 这 是 因为 
所 有 的 粒子 都 用 于 表示 目标 “出 现 ”， 而 没有 表示 目标 “消失 ”的 粒子 。 

另外 ， 在 检测 前 跟踪 应 用 中 ， 计 算 粒 子 权 重 时 采用 的 是 似 然 比 形式 而 非 传统 滤波 应 
用 中 的 似 然 形 式 ， 即 

Ki xz) Š Pet G PD) =exp L a @% XE Mi s | (9-36) 
per) 20 
于 是 ， 对 于 第 s(s=1,2…,N) 个 粒子 ， 其 权 值 可 表示 为 


H II i” lw) gs 
e| E. (9-37) 


iC; (t) jeC (xp) 
| E:=0 
完整 的 检测 前 跟踪 算法 流程 如 下 。 
Alg (9.3): MPF based Track-before-Detect 
UE: 初始 化 
在 k=0 时 刻 ， 按 照 下 面 的 伪 码 获取 初始 粒子 集 (xo 0 Y. (xa, B.) : 
efor s-EN 
—x0" ~U[0,n,], yo  -U[0,n, ] 
I UU RR 
xp - po yn) por 
send for 
RP: U 表示 均匀 分 布 ， Tuis 和 了 表示 目标 信号 强度 的 可 能 范围 。 同 时 根据 目标 存在 
的 先 验 概率 PE, =1) 随机 地 选择 一 部 分 粒子 使 其 状态 置 为 空 ， 即 x) =@ 。 
对 于 k 宇 1， 采 用 以 下 步 又 递 推 。 
步骤 2: 权 值 计算 
通过 式 (9-36) MIA (9-370 HARE wO HA-HA DP. 
步骤 3: 测量 更 新 
对 非 线性 部 分 ， 通 过 粒子 重 采 样 使 得 PrO = xpi) - up? 7 ， 并 得 到 新 的 粒子 集 为 
(xV; 对 线性 部 分 直接 通过 上 一 时 刻 的 估计 获得 更 新 ， 即 LT Sar P, = Bae 
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步骤 4: 时 间 更 新 
非 线性 部 分 通过 以 下 算式 预测 新 的 粒子 : 
x ~ N [xT Ax À +B, QO! + ALPLCAL) ) 

线性 部 分 则 通过 卡尔 曼 滤波 公式 计算 : 

S; = BS +K, (EY — Ax? — Biz) 

P, = CPC! +Q, - K,M,K, 

M,-BP,B' +Q; 

K,=CP,B' M,' 
步骤 5: 检测 概率 计算 
统计 代表 目标 存在 〈 Ee)=1) 的 粒子 数 N. ， 由 此 得 到 检测 概率 的 估 值 户 =N,/N ; 若 

该 值 大 于 门限 已, 表示 检测 到 目标 ， 则 进入 步骤 6; 反之 ， 直 接 转 到 步骤 7。 

步骤 6: 状态 估计 


N N N N 

An. i ny) i al _ i LG) i 

$ = 2 Exp 0)/3 E, £-» Ex /* E, 
i=l i=l i=l i-l 


步骤 7: 令 上 = 上 +1， 重 复 步骤 2 一 步骤 5 
仿真 中 ， 假 定 目标 初始 位 置 为 (4.$,7.2) ， 初 始 速度 为 (0.25,0.1) ， 初 始 时 刻 的 信和 号 强 
度 石 根据 信 噪 比 SNR RAE, QU=leAl,, Qi=le-4L ， 其 他 参数 设置 见 表 9.1。 
表 9.1 MPF-TbD 滤波 算法 参数 设置 
xa se [^ L7] 
| $m | o | i | | 3 | o | os | | 


设 总 共 进 行 40 个 采样 时 间 的 观测 (产生 40 帧 数据 ), 开始 时 监视 区 内 无 目标 ,第 k=6 
时 刻 目标 开始 出 现 并 持续 到 k=35 时 刻 ， 之 后 目标 消失 。 在 粒子 数目 W=1000 ， 信 噪 比 
SNR=6dB,9dB,12dB 条 件 下 ， 分 别 进行 200 次 实验 ， 所 得 结果 如 图 9.6 一 图 9.8 所 示 。 





(b) 9dB (c) 12dB 


图 9.6 不 同 信 骂 比 条 件 下 的 图 像 


图 9.6 给 出 的 是 不 同 信 品 比 条 件 下 的 第 15 帧 图 像 ， 可 见 通 过 肉眼 很 难看 出 是 否 存 
在 目标 , 更 无 法 确定 目标 位 于 哪个 位 置 , 图 9.7 是 通过 边缘 化 粒子 滤波 得 到 的 目标 检测 
结果 。 
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? 和 TOSEUEHENS 
ef 


Probability of target detection 





0 5 10 15 20 25 30 35 40 
Time index 


图 9.7 不 同 信 骂 比 条 件 下 的 检测 概率 


由 图 9.7 中 的 结果 可 以 看 出 , 随 着 信 品 比 的 提高 , 目标 检测 概率 也 有 不 同 程度 的 提高 ; 
当 信 噪 比 大 于 6dB 时 ， 对 目标 出 现 的 检测 有 2 个 时 刻 的 时 延 ， 而 当 信 品 比 为 6dB 时 的 检 
测 时 延 为 3 个 时 刻 ， 在 不 同 信 品 比 条 件 下 ， 对 目标 消失 的 检测 均 存 在 1 个 时 刻 的 时 延 。 

9.8 和 图 9.9 进一步 给 出 了 不 同 信 品 比 下 的 目标 位 置 和 速度 估计 ARMSE, 为 了 评 
价 的 客观 性 ， 只 对 检测 到 目标 的 情况 下 (时 刻 9 一 时 刻 35) 进行 统计 。 由 图 中 的 结果 可 
见 ， 随 着 信 品 比 的 提高 ， 检 测 前 跟踪 的 状态 估计 精度 也 进一步 提高 ， 不 过 ， 当 信 品 比 大 
到 一 定 程 度 后 (如 大 于 14dB)， 信 和 噪 比 的 提高 对 估计 性 能 的 改善 不 再 明显 (说明 在 高 信 
噪 比 条 件 下 影响 TbD 性 能 的 主要 因素 已 不 再 是 检测 结果 )。 


---@-- axis-x 
一 日 一 AXis-y 


ARMSE of position estimation 





6 8 10 12 14 16 
SNR(dB) 


图 9.8 ”位置 估计 平均 均 方 根 误差 


ARMSE of velocity estimation 





` 6 8 10 12 14 16 
SNR(dB) 


图 9.9 速度 估计 平均 均 方 根 误差 
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由 此 可 见 ， 采 用 边缘 化 粒子 滤波 技术 可 实现 对 低 信 品 比 条 件 下 目标 检测 前 跟踪 。 当 
然 ， 上 述 例子 给 出 的 只 是 一 般 的 实现 过 程 ， 实 际 中 为 了 得 到 更 好 的 检测 前 跟踪 性 能 ， 可 
能 需要 辅 以 其 他 的 手段 ， 如 对 非 线性 部 分 进行 粒子 更 新 时 可 融入 观测 量 信息 ， 并 在 检测 
环节 增加 特殊 的 策略 如 引入 序 贯 检测 所 等 以 提高 对 低 信 噪 比 条 件 的 适应 能 力 。 


94 7k 3g 


尽管 粒子 滤波 算法 在 处 理 非 线性 问题 中 具有 普遍 的 适用 性 ， 但 其 估计 性 能 依赖 于 粒 
子 对 状态 后 验 分 布 的 近似 程度 。 在 给 定 的 估计 性 能 要 求 下 ， 算 法 所 需 的 粒子 数 随 待 估 状 
态 维 数 的 增加 呈 指 数 增长 ， 使 得 这 一 技术 在 很 多 应 用 中 受到 限制 。 但 在 实际 应 用 中 ， 很 
多 非 线性 系统 模型 都 具有 线性 的 子 结构 ， 在 这 种 条 件 下 采用 “边缘 化 ”技术 将 模型 分 解 
成 线性 和 非 线 性 两 个 子 结构 ， 使 粒子 滤波 器 只 工作 在 低 维 状态 空间 ， 可 提高 滤波 算法 的 
效 费 比 。 

本 章 主要 阐述 “边缘 化 ”粒子 滤波 的 算法 原理 ， 并 给 出 了 几 种 典型 系统 模型 的 算法 
实现 过 程 , 在 此 基础 上 结合 两 个 不 同 的 目标 跟踪 实例 介绍 了 边缘 化 粒子 滤波 的 具体 应 用 。 
需要 指出 的 是 ， 在 相同 的 粒子 数目 条 件 下 ，MPF 的 运算 量 要 低 于 传统 基于 SIS 技术 的 粒 
子 滤波 器 (如 前 面 讨论 的 EPF. UPF 等 )， 但 通常 要 高 于 SPF 的 运算 开销 。 因 此 ， 在 实 
际 应 用 中 , 应 根据 具体 的 问题 形式 及 估计 性 能 指标 要 求 等 ， 综 合 分 析 不 同 算法 的 效 费 比 ， 
从 而 完成 对 算法 的 合理 选择 。 
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第 10 章 智能 优化 采样 滤波 


第 7 章 中 指出 ， 标 准 粒子 滤波 算法 尽管 在 实现 上 较为 方便 ， 但 由 于 仅 利 用 了 先 验 分 
布 信息 ， 存 在 严重 的 粒子 退化 现象 及 近似 效率 低下 问题 。 第 8 章 从 不 同 的 角度 阐述 了 基 
于 建议 分 布 函数 的 粒子 采样 方法 ， 以 此 来 提高 滤波 器 的 估计 性 能 。 在 这 类 方法 中 ， 主 要 
通过 融入 量 测 信息 提高 粒子 的 产生 质量 ， 从 而 改善 对 状态 后 验 分 布 的 近似 效果 。 

事实 上 ， 导 致 标准 粒子 算法 效率 低下 的 主要 原因 之 一 是 粒子 退化 问题 ， 若 能 采用 某 
种 手段 减轻 或 消除 这 一 现象 ， 则 可 以 起 到 提高 粒子 滤波 估计 性 能 的 目的 。 遗 传 算法 是 模 
拟 自 然 界 生物 进化 过 程 与 机 制 的 一 种 随机 优化 与 搜索 方法 ， 它 反映 的 是 一 种 进化 思想 ， 
这 种 思想 为 解决 粒子 退化 问题 提供 了 一 条 重要 思路 。 由 于 遗传 算法 本 质 上 是 一 种 智能 优 
化 方法 ， 推 而 广 之 ， 其 他 智能 优化 方法 如 人 工 免疫 算法 、 人 工 鱼 群 算法 及 粒子 群 优 化 算 
法 等 也 具备 这 种 潜能 。 

本 章 主要 阐述 基于 智能 优化 采样 的 粒子 滤波 算法 ， 重 点 介绍 遗传 算法 、 免 疫 算法 和 人 
工 鱼 群 算法 的 基本 原理 及 与 粒子 滤波 框架 的 结合 方法 , 分 别 得 到 遗传 粒子 滤波 (GPF 20257. 
人 工 免疫 粒子 滤波 (AIPF) 中 全 和 人 工 鱼 群 粒 子 滤波 (AFSPF) [6 算法 。 在 GPF 算法 
中 ， 主 要 是 通过 选择 、 交 叉 、 变 异 等 操作 代替 传统 的 重 采 样 方 法 ， 依 此 防止 粒子 的 退化 现 
象 ; 在 AIPF 算法 中 ， 将 粒子 看 作 是 抗体 ， 应 用 免疫 算法 的 优化 机 理 在 粒子 更 新 过 程 中 对 
每 个 抗体 进行 克隆 ， 从 而 得 到 优选 的 抗体 ， 保 持 样本 集 的 多 样 性 ， 而 在 AFSPF 算法 中 ， 
将 人 工 鱼 群 的 个 体 视 为 粒子 ， 通 过 将 况 食 、 聚 群 行为 与 粒子 更 新 相 结 合 ， 使 得 先 验 粒子 向 
高 似 然 区 域 移动 ， 从 而 改善 粒子 的 分 布 。 


10.1 遗传 粒子 滤波 


遗传 算法 (GA ) 是 模拟 自然 界 生 物 进 化 过 程 与 机 制 来 求解 最 优化 问题 的 一 类 自 组 织 、 
自 适 应 人 工 智 能 技术 。 它 通过 模拟 达尔 文 “ 优 胜 劣 汰 、 适 者 生存 ”的 原理 激励 好 的 结构 ， 
同时 寻找 最 优 结构 ， 是 一 种 随机 的 优化 与 搜索 方法 。 和 遗传 算法 反映 的 是 一 种 进化 思想 ， 
而 粒子 滤波 器 中 存在 的 主要 问题 是 粒子 退化 , 用 遗传 进化 的 思想 来 改善 粒子 的 分 布 特性 ， 
可 有 效 提高 粒子 滤波 的 估计 效果 。 


10.1.1 ”遗传 算法 基本 原理 


GA 是 一 种 基于 自然 选择 和 自然 遗传 原理 的 随机 搜索 技术 ， 通 过 选择 、 交 叉 和 变异 
实现 种 群 的 更 新 和 优化 。 遗 传 算 法 每 次 迭代 都 保存 一 个 最 优 种 群 ， 而 不 仅仅 是 一 个 最 优 
个 体 。 在 给 出 具体 的 算法 前 ， 首 先 介绍 一 下 GA 涉及 的 一 些 重要 概念 ， 见 表 10.1。 
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表 10.1 遗传 算法 基本 术语 表 


遗传 因子 
也 称 为 基因 ， 是 构成 染色 体 的 基本 单位 ， 即 个 体 编码 中 的 每 一 位 


色 
生物 细胞 中 含有 一 种 微小 的 丝 状 化 合 物 ， 它 是 遗传 物质 的 主要 载体 ， 由 多 个 遗传 因子 一 基因 组 成 


组 成 种 群 的 解 空间 中 的 一 个 解 。 遗 传 算法 的 中 心思 想 就 是 通过 遗传 操作 从 母体 中 找 出 优良 的 个 体 并 使 
population | 优良 的 状态 遗传 保持 到 下 一 代 


解 空 间 的 一 组 作为 父 代 遗传 用 的 初始 解 


遗传 操作 通常 不 能 直接 处 理 数值 变量 ， 只 能 针对 编码 串 进行 ， 这 样 ， 就 需要 将 数值 变量 转化 为 一 个 二 
Codi 进 制 编码 串 ， 这 个 步 又 就 是 编码 


Fitness | 境 的 适应 程度 。 在 遗传 算法 中 ， 用 适应 度 来 评价 个 体 优 劣 程度 

选择 指 决定 以 一 定 的 概率 从 种 群 中 选择 若干 个 体 的 操作 。 一 般 而 言 ， 选 择 的 过 程 就 是 基于 适应 度 的 优胜 劣 
Selection | 汰 过 程 ， 某 个 体 被 选择 的 概率 与 其 适应 度 值 成 正比 

从 种 群 中 随机 选取 两 个 个 体 做 父 体 和 母体 ， 再 依 交叉 概率 随机 地 选取 一 交叉 位 ， 然 后 将 两 个 体 中 位 于 
交叉 位 后 的 符号 串 互 换 ， 保 留 交叉 位 前 的 各 基因 位 符号 不 变 ， 形 成 两 个 新 个 体 的 遗传 操作 ， 这 个 过 程 又 
称 为 基因 重组 ， 俗 称 “ 杂 交 ”， 它 是 获取 优良 个 体 的 重要 手段 

对 个 体 编码 中 的 每 一 位 基因 位 根据 变异 概率 进行 0、! 翻转 ， 形 成 新 个 体 的 遗传 操作 。 变 异 操作 可 使 适 
应 值 小 的 个 体 发 生变 异 或 在 整个 群体 性 态 趋 于 一 致 时 人 迫使 个 体 发 生变 化 ， 同 时 ， 防 止 适应 值 大 的 个 体 变 
异 ， 从 而 使 每 一 代 种 群 保持 新 鲜 个 体 ， 避 免 种 群 进化 停 湛 而 过 早 收 敛 


在 GA 的 运算 过 程 中 ， 通 常 不 对 所 求解 问题 的 实际 决策 变量 直接 进行 操作 ， 而 是 对 
表示 可 行 解 的 个 体 编码 施加 选择 、 交 叉 、 变 异 等 遗传 运算 。 因 此 ，GA 一 般 都 离 不 开 编 
码 和 解码 问题 ， 常 用 的 编码 方法 是 二 进 制 编码 方法 。 同 时 ， 针 对 某 个 具体 问题 ， 编 码 和 
解码 操作 也 会 增加 算法 实现 的 复杂 度 。 若 能 直接 针对 实数 进行 遗传 操作 、 避 免 编 解码 过 
程 ， 则 可 有 效 降 低 算法 实现 的 复杂 度 。 

GA 是 一 个 迭代 过 程 ， 同 时 也 是 一 个 概率 搜索 算法 。 假 设 第 k 代 种 群 为 G, = (X, 
pe)... PO}, Jeri, N 表示 该 种 群 中 的 个 体 数 (也 即 染 色 体 数 )， 则 利用 迭代 方法 ， 通 
过 选择 、 交 叉 、 变 异 操作 就 能 得 到 上 +1 代 种 群 G = (PO. Bes POS) 。 具 体 过 程 可 概 
括 为 以 下 三 个 步骤 。 

步骤 1: k=0 

初始 化 : 随机 产生 一 组 初始 解 ， 作 为 初始 种 群 及 wmw={PRO,i=1L2…,N} ° 

个 体 评价 : RRR P my 中 每 一 个 体 的 适应 度 。 

步骤 2: k-k^l 

选择 操作 :对 种 群 P o 进行 选择 得 到 种 群 及 ivw。 

交叉 操作 : 对 种 群 甩 ,i us 中 的 个 体 进行 交叉 运算 。 

变异 操作 : 对 种 群 Bw) 中 的 个 体 进行 变异 运算 。 


变异 


Mutation 
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种 群 评估 ， 评 估计 算出 的 种 群 名 ,0 ° 
步骤 3: 终止 条 件 判断 
若 进化 代数 大 《迭代 次 数 ) 小 于 设 定 值 或 代价 函数 大 于 某 一 门限 值 ， 转 到 步骤 2。 
在 上 述 几 大 步骤 中 ， 遗 传 进化 运算 的 具体 实现 方法 如 下 ; 
CD 初始 化 只 。 由 先 验 概率 分 布 p( 及 ) 随机 得 到 初始 化 群体 只 =PO, H 
~ p(R)) ° 
(2) 个 体 评价 BO 。 个 体 评价 即 计算 该 种 群 中 每 一 个 体 (染色 体 ) 的 适应 度 uD ,可 
以 通过 适应 度 函 数 APO) 来 计算 ， 该 函数 通常 可 根据 实际 需要 来 定义 ， 如 
up = fid) =| -KE9|。 因 此 ， 种 群 中 个 体 的 适应 度 为 up = Ar(BOJXE=12…,ND 。 
(3) 选择 操作 。 新 种 群 有， 可 以 按照 选择 概率 从 上 一 代 种 群 B_, ,中 选择 得 到 ， 有 
很 多 选择 方法 ， 转 盘 操作 方法 就 是 其 中 的 一 种 。 
利用 适应 度 函数 ， 可 得 每 一 个 体 的 概率 a, 为 
ap, = /9 wD,, ILN 
ja 
由 此 得 到 每 一 个 体 PO 的 累积 概率 cD, 3 
c, = Yap, i=1,2,…,N 


在 区 间 [0,1] 内 随机 产生 一 个 随机 数 ， 即 w~U[0,1] ， 转 动 转盘 ， 按 照 下 列 规则 选择 一 
个 个 体 作 为 下 一 代 种 群 中 的 新 个 体 : 

若 x<co ， 选 择 第 1 个 个 体 PO, FE P = PO ; 

Puz ， 选 择 第 j 个 个 体 , HD < 和 co ， 令 PO SPD, i=12-. N. 

(4) 交叉 操作 。 设 交叉 概率 为 ， 产 生 随 机 数 u~U[0,1] 。 若 u < P ， 从 种 群 PB m 中 
随机 选择 两 个 个 体 {P"”,P"} ， 按 照 下 式 进行 配对 ， 并 用 新 个 体 {Px ,Px } 代替 旧 个 体 
{RMR} o 

PP =ap™ +(1-a)P” 
PP =aP” «(1- a)? 
式 中 :we[0,1] ， 其 大 小 可 根据 具体 问题 而 定 。 
(5) 变异 操作 。 假 设 变异 概率 为 Pp, ， 产 生 随机 数 x~W[0,1] 。 若 zx < P, AERE P o 
中 随机 选择 一 个 个 体 PW ， 按 照 下 式 进 行 变异 ; 
Pr =P +n 
式 中 
n~ N(0,2) 

由 上 面 的 分 析 中 可 以 看 出 ， 交 叉 、 变 异 等 操作 都 是 直接 对 实数 进行 的 ， 这 样 可 避免 
传统 遗传 方法 中 的 二 进 制 编码 和 解码 操作 环节 ， 因 此 可 以 根据 实际 需要 自 定义 交叉 、 变 
异 操作 算 子 ， 更 方便 于 算法 的 实现 。 另 外 ， 仔 细 分 析 不 难 发 现 ，GA 算法 与 前 文 所 述 的 
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粒子 滤波 算法 无 论 在 模型 还 是 运算 过 程 中 都 具有 明显 的 相似 性 ， 两 者 之 间 的 类 比 关 系 如 
K 10.2 和 图 10.1 MFR. 


表 10.2 粒子 滤波 算法 与 遗传 算法 模型 比较 

















图 10.1 粒子 滤波 与 遗传 算法 执行 过 程 类 比 


10.1.2 ”遗传 粒子 滤波 算法 


遗传 粒子 滤波 (GPF) 算法 主要 是 将 GA 中 的 选择 、 交 叉 、 变 异 等 进化 思想 引入 到 
粒子 滤波 算法 中 ， 代 替 传 统 的 重 采样 方法 。 由 于 GA 具有 独特 的 寻 优 能 力 ， 可 以 提高 粒 
子 的 使 用 效率 ， 使 逼近 后 验 概率 分 布 最 大 值 所 需 的 粒子 数 大 大 减少 ;同时 ， 由 于 GA 能 
有 效 增加 粒子 的 多 样 性 ， 可 有 效 防止 出 现 滤波 发 散 现象 ， 提 高 状态 估计 精度 。 

GPF 算法 中 的 选择 、 交 叉 、 变 异 操作 实现 步骤 描述 如 下 。 

(1) 选择 操作 。 对 大 时 刻 粒子 集 (xD uw YN. w 可 视 为 此 粒子 集中 对 应 粒子 的 适 
应 度 ， 适 应 度 的 方差 对 应 粒子 重要 性 权 值 的 方差 。 由 于 粒子 重要 性 权 值 的 方差 最 小 时 ， 
状态 估计 最 接近 真实 状态 。 因 此 ， 根 据 适 应 度 的 方差 大 小 来 判断 选择 操作 。 若 方差 大 小 
符合 要 求 ， 进 行 选 择 操作 ， 保 留 所 有 粒子 ， 而 不 再 进行 交叉 、 变 异 操作 ， 直 接 进 入 预测 
阶段 ; 车 方 差 大 小 不 符合 要 求 ， 跳 过 选择 操作 ， 直 接 进 行 交 叉 、 变 异 操作 。 

(2) 交叉 操作 。 从 粒子 集中 随机 选取 两 个 粒子 (xP) x) ,1 ， 按 照 下 式 进行 交叉 
操作 。 

x”) 2 ax? - (1-2 a)x +n 
x? = ax? - (12 a)x? +g 
AP: n- N 0502) 为 正 态 分 布 ，w ~ U(0,1) 为 均匀 分 布 。 
(3) 变异 操作 。 从 粒子 集中 随机 选择 一 个 粒子 APP ， 按 照 下 式 进行 变异 操作 : 


s= xP +n 
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变异 规则 为 : WR perk ppe”) ， 则 接受 粒子 站， 否则， 产生 wu~U(0,1)， 并 
判断 zx < p(z X?) )/p(zsx2) 是 否 成 立 ， 成 立时 接受 粒子 x ， 不 成 立 则 接受 粒子 x? 。 
HFA 10-1) AR 〈10-2) 所 描述 的 非 线性 系统 
Xen = f(x) + W, (10-1) 
= h(x,) + v, (10-2) 
结合 选择 、 交 叉 和 变异 的 操作 过 程 ， 可 将 GPF 算法 总 结 如 下 。 
Alg (10.1): Genetic Particle Filtering 
步骤 1: 初始 化 
由 先 验 概率 p(x,) 产生 粒子 集 (xO) ， 并 令 所 有 粒子 权 值 为 VN 。 
步骤 2: 权 值 更 新 
在 大 时 刻 ， 计 算 粒 子 权 值 
ug =w, p, xp )p(x ex) j-12,-.N 
qx | x 2.) ns 
并 且 归 一 化 
g” = wP rS 人 
izi 


步骤 3: 遗传 操作 

(D 选择 操作 。 计 算 粒 子 权 值 方差 ， 若 方差 符合 要 求 ， 保 留 粒 子 为 {X,Y ， 转 
到 步骤 4;， 否则 ， 直 接 进 行 下 一 步 操作 。 

(2) 采用 前 文 所 述 的 方法 进行 粒子 的 交叉 、 变 异 操作 ， 从 而 得 到 新 的 粒子 集 
全 ,下 ji。 


步骤 4: 估计 输出 
y 
状态 估计 : <&, a zO 


协 方差 估计 : P= -> üf (x -EO 


步骤 S. 状态 预测 
利用 状态 方程 f C) 预测 下 一 时 刻 的 粒子 xp, BU 
xi = fp tw, i=1,2,...N 
步骤 6: 时 刻 上 = 大 +1， 转 到 步骤 2 
需要 说 明 的 是 ， 在 步骤 2 中 ，4(x xPIxp.. z) 为 产生 粒子 的 建议 分 布 ， 通 常 可 选择 为 


状态 的 先 验 分 布 即 pos? x.) 。 


10.2 人工 免疫 粒子 滤波 


免疫 优化 是 受 生物 免疫 机 制 启发 而 形成 的 一 种 新 型 的 算法 ， 它 模拟 了 生物 免疫 系 
统 产生 抗体 抵御 外 来 入 侵 抗 原 的 自 适应 能 力 ， 可 实现 入 侵 抗原 识别 、 多 样 抗体 生成 、 
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自我 调节 、 免 疫 记 忆 等 功能 。 生 物 免 疫 系统 虽然 复杂 ， 但 是 其 呈现 出 来 的 抵御 抗原 的 
自 适应 能 力 却 是 十 分 明显 的 。 人 工 免 疫 算法 是 一 种 基于 人 工 免 疫 系统 的 随机 全 局 搜索 
算法 ， 具 有 全 局 多 样 性 寻 优 能 力 。 将 人 工 免疫 算法 的 寻 优 能 力 与 粒子 滤波 算法 相 结合 ， 
可 从 众多 粒子 中 精 选 出 较 好 的 粒子 ， 改 善 样本 集 的 多 样 性 ， 从 而 提高 粒子 滤波 的 估计 
能 力 。 


10.2.1 人工 免疫 算法 原理 


免疫 优化 算法 通过 模拟 免疫 系统 的 抗体 多 样 性 保持 机 制 ， 实 现 免疫 系统 的 自我 调节 
功能 ， 即 利用 抗体 的 浓度 选择 机 制 ， 实 现 对 抗体 的 促进 和 抑制 作用 ， 保 持 抗体 的 多 样 性 。 
为 了 更 好 地 理解 免疫 算法 的 基本 原理 ， 首 先 对 几 个 重要 概念 作 一 简要 介绍 。 

l|. B 细胞 

B 细胞 是 B 淋巴 细胞 的 简称 。B 细胞 是 体内 唯一 能 够 产生 抗体 (免疫 球 蛋 白 分 子 ) 
的 细胞 ， 其 表面 含有 可 以 识别 特异 性 抗原 的 多 种 抗体 分 子 ， 多 样 性 来 自 千 百 万 种 不 同 的 
B 细胞 克隆 。 每 一 个 B 细胞 克隆 的 特性 是 由 其 遗传 性 决定 的 ， 可 产生 一 种 能 与 抗原 特异 
相 结 合 的 免疫 球 和 蛋白 分 子 。B 细胞 有 三 个 主要 功能 : 产生 抗体 、 提 呈 抗 原 和 分 泌 细 胞 因 
子 参与 免疫 调节 。 

2. 抗原 

在 现代 免疫 学 中 ,抗原 (Antigen, Ag) 是 指 能 够 诱导 免疫 系统 发 生 免疫 应 答 并 能 与 
其 产生 的 抗体 或 效应 细胞 在 体内 或 体外 发 生 特 异性 反应 的 物质 ， 抗 原 表 面 被 抗体 识别 的 
部 分 称 为 抗原 决定 基 。 抗 原 对 应 免疫 算法 中 的 目标 函数 。 

3， 抗 体 

抗体 (Antibody，Ab) 是 B 细胞 识别 抗原 后 克隆 扩 增 分 化 为 浆 细胞 所 产生 的 一 种 蛋 
白质 分 子 ， 也 称 为 免疫 球 和 蛋白 分 子 。 抗 体 结合 由 外 部 入 侵 的 感染 微 组 织 或 者 有 毒物 等 抗 
原 ， 然 后 依靠 自己 或 者 借助 免疫 系统 其 他 元 素 帮 助 破坏 这 些 抗原 ， 消 除 对 人 体 的 威胁 。 
抗体 由 抗体 决定 基 和 独特 型 组 成 ， 抗 体 决 定 基 是 抗体 上 识别 抗原 决定 基 的 部 分 ， 而 独特 
型 是 抗体 上 可 供 自身 免疫 细胞 识别 的 抗原 决定 基 。 抗 体 对 应 目标 函数 的 输入 变量 。 

4， 亲 和 度 

免疫 系统 中 的 免疫 识别 是 发 生 在 分 子 水 平 的 ， 并 且 这 种 识别 是 基于 抗体 决定 基 和 抗 
原 决定 基 之 间 的 形状 互补 。 发 生 免 疫 识 别 的 抗体 决定 基 和 抗原 决定 基 在 结构 上 越 互 补 ， 
结合 就 越 可 能 发 生 ， 结 合 的 力度 也 就 越 紧密 ， 这 种 结合 的 力度 与 强度 被 称 为 抗体 与 抗原 
之 间 的 亲 和 度 (affinity)。 亲 和 度 对 应 最 优化 问题 中 的 函数 代价 /测度 值 。 

5.， 亲 和 度 成 熟 

随 着 抗体 分 子 与 同 种 抗原 的 多 次 遭遇 ， 记 忆 应 答 中 的 抗体 与 较 早 的 初始 应 答 中 的 抗 
体 相 比 具 有 更 高 的 平均 亲 和 度 ， 这 个 现象 称 为 免疫 应 答 的 亲 和 度 成 熟 。 由 此 可 见 ， 免 疫 
应 答 的 亲 和 度 成 熟 要 求 成 熟 应 答 中 的 抗体 分 子 的 抗原 结合 点 在 结构 上 与 在 初次 应 答 中 抗 
体 分 子 的 抗原 结合 点 不 同 。 

人 工 免疫 算法 适合 解决 目标 优化 任务 ， 尤 其 是 多 峰值 目标 优化 问题 ， 其 原理 框图 如 
图 10.2 所 示 。 
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抗原 定义 


抗体 初始 化 


结 







图 102 免疫 算法 原理 框图 


根据 上 述 原 理 图 ， 可 得 人 工 免疫 算法 (AIA) 的 具体 流程 如 下 。 

Alg (10.2): Artificial Immune 

步骤 1: 建立 目标 函数 j() ， 即 引入 抗原 。 

由 于 多 目标 优化 问题 最 终 可 以 转化 为 一 个 待 优 化 《最 大 化 或 最 小 化 ) 的 目标 函数 
f()， 故 在 抗原 种 群 4g 中 只 包含 一 个 抗原 。 

步骤 2: 令 k=0 (大 为 种 群 的 代数 )， 随 机 产生 N 个 初始 抗体 ， 构 成 初始 抗体 种 群 
Ab, = (Ab(O", 。 这 样 , 对 于 一 个 特定 的 抗体 可 以 计算 得 到 一 个 相应 于 目标 函数 (抗原) 
的 亲 和 度 抗体 种 群 Ab, ww 中 的 每 个 抗体 ADD 表示 输入 空间 的 一 个 元 素 )。 

步骤 3: 克隆 选择 。 

QD 确定 抗体 种 群 Ab, ww 中 所 有 N 个 抗体 关于 抗原 (目标 函数 MAIR af 。 

D 从 Ab, m 中 选择 N, 个 亲 和 度 最 高 的 抗体 构成 一 个 新 的 关于 抗原 的 高 亲 和 度 抗体 
集合 Ab, 7 b yo ° 

@ 对 步骤 39 PHA TERT DUAE FERE Ab, v 中 的 抗体 分 别 进行 复制 。 

由 步骤 3@ 选 择 的 各 抗体 的 克隆 个 数 与 其 相应 于 抗原 的 亲 和 度 成 正比 ， 即 对 于 Ni 个 
所 选 抗体 , 与 抗原 亲 和 度 越 高 的 抗体 其 克隆 的 个 数 越 多 , 从 而 产生 一 个 克隆 抗体 种 群 C1 。 

步骤 4: 抗体 库 更 新 。 

O 克隆 变异 。 克隆 抗体 在 分 裂 的 过 程 中 会 发 生 随 机 点 变异 , 且 其 中 的 每 个 抗体 发 生 
变异 的 概率 与 其 抗原 亲 和 度 成 反比 , 即 克 隆 抗体 中 与 抗原 亲 和 度 越 高 的 抗体 变异 率 越 低 ， 
产生 一 个 变异 的 克隆 抗体 种 群 C7 。 

D 亲 和 度 计算 。 确 定 变异 克隆 抗体 种 群 C7 中 每 个 抗体 关于 抗原 的 亲 和 度 aff" 。 


159 


© 克隆 选择 。 从 变异 克隆 种 群 C7 中 重新 选择 N 个 亲 和 度 最 高 的 抗体 Ab, os ° 

Ab, y 作为 候选 抗体 进入 记忆 抗体 种 群 ， 这 里 Abi (为 克隆 扩 增 抗体 种 群 。 

© 受 体 编辑 。 模 拟 生物 免疫 系统 的 受 体 编辑 过 程 随机 产生 N, 个 全 新 抗体 构成 新 抗 
体 种 群 Ab, (w,)， 并 用 Ab, 4w,) 中 的 入 ,个 新 抗体 替换 Ab, o 中 N, 个 亲 和 度 较 低 的 抗体 构 
成 下 一 代 抗 体 种 群 Ab, on ° 

步骤 5: 种 群 评估 。 

计算 种 群 Ab... a 的 平均 亲 和 度 ; 

(2) 判断 抗体 库 是 否 满足 成 熟 条 件 ， 若 已 成 熟 则 转 步 骤 7， 和 否则 转 步 骤 6。 

步骤 6: Ek-kd, XR 3 一 步骤 S. 

步骤 7: 当 满足 迭代 终止 条 件 时 结束 循环 。 

循环 终止 的 条 件 应 满足 使 抗体 种 群 的 平均 亲 和 度 达到 一 个 稳定 值 。 这 时 ， 从 循环 碗 
代 最 终 产 生 的 抗体 种 群 中 选择 出 亲 和 度 最 高 的 抗体 作为 最 优 解 。 


10.2.3 ”人 工 免疫 粒子 滤波 算法 


通过 前 文 的 分 析 可 知 ， 用 于 表示 外 来 侵犯 的 抗原 和 免疫 系统 产生 的 抗体 分 别 与 待 求 
解 问题 的 目标 函数 和 候选 解 相 对 应 。 基 于 这 一 原理 ， 若 将 被 估计 的 状态 与 抗原 相对 应 ， 
用 于 状态 近似 的 粒子 与 抗体 相对 应 ， 即 可 实现 免疫 算法 与 粒子 滤波 的 结合 ， 得 到 人 工 免 
疫 粒子 滤波 算法 ， 具 体 思 路 如 下 : 

根据 重要 密度 函数 随机 抽取 N 个 样本 作为 初始 抗体 , 这 N 个 抗体 实际 上 也 是 状态 估 
计时 所 要 用 到 的 粒子 ; 对 这 N 个 粒子 进行 更 新 ， 分 别 计 算 N 个 抗体 的 亲 和 度 和 抗体 与 抗 
体 之 间 的 排斥 力 ， 按 照 亲 和 度 的 大 小 确定 每 个 抗体 应 克隆 的 抗体 数目 。N 个 初始 抗体 和 
克隆 所 得 的 所 有 抗体 组 成 新 的 抗体 群 ， 通 过 其 亲 和 度 的 计算 ， 可 舍弃 相似 的 抗体 ， 并 从 
中 选 出 最 好 的 N 个 抗体 用 于 对 下 一 时 刻 的 状态 估计 。 这 种 做 法 必然 会 使 计算 量 有 所 增 
加 ， 但 在 条 件 允 许 的 情况 下 ， 可 使 样本 集 保持 一 定 的 多 样 性 ， 减 轻 退 化 现象 的 影响 ， 将 
优选 后 的 粒子 用 于 后 验 概率 密度 的 近似 ， 可 有 效 提高 状态 估计 性 能 。 

若 以 1 和 粒子 集 (xO wp) 分 别 作为 抗原 和 抗体 ， 可 得 到 如 下 几 个 重要 定义 。 

定义 1 时刻 第 i 个 抗体 的 亲 和 度 aff (i) 定 义 为 

aff, (i)-1— wf? (10-3) 
aff. (i) 越 小 ， 表 明 抗 体 和 抗原 匹配 得 越 好 。 

定义 2 时 刻 第 i 个 抗体 与 第 j 个 抗体 的 排斥 力 off.(i, 站 定义 为 

off, G, j- |x - xP | (10-4) 
off.(i, 站 越 小 ， 表 明 第 i 个 抗体 与 第 j 个 抗体 越 相 似 。 

定义 3 时刻 第 i 个 抗体 的 克隆 数 knum (7) 定义 为 


knum, (iy-round[ » cos xa o) (10-5) 
式 中 : round() 表示 向 最 近 的 整数 取 整 。 选 用 余弦 函数 的 目的 是 为 使 亲 和 度 小 的 粒子 克隆 


160 


数目 较 大 ， 并 且 分 布 在 0~N 之 间 。 殉 隆 的 原则 是 促进 亲 和 度 小 的 抗体 ， 而 抑制 亲 和 度 大 
的 抗体 ， 以 使 系统 能 快速 收敛 于 全 局 最 优 解 。 

定义 4 时刻 第 i 个 抗体 的 变异 规则 定义 为 

xQ = x? +af (i) -C (10-6) 

AP: CN (69,22) 为 服从 高 斯 分 布 的 随机 量 。 变 异 的 原则 是 对 亲 和 度 小 的 抗体 赋予 较 
小 的 变异 量 ， 而 对 亲 和 度 大 的 抗体 则 赋予 较 大 的 变异 量 。 

由 此 可 得 免疫 粒子 滤波 (AIPF) 算法 流程 如 下 。 

Alg (10.3): Artificial Immune Particle Filtering 

步骤 1: 初始 化 。 在 k=0 时 刻 , 根据 重要 密度 函数 抽样 出 六 个 粒子 , BD (xO) UNS 。 

步骤 2. 更 新 。 估 计 大 时 刻 的 粒子 xD 及 其 归 一 化 权 值 ， 并 将 这 N 个 粒子 作为 初始 抗 
体 群 存 入 记忆 库 。 

步骤 3: 计算 亲 和 度 。 按 照 式 《10-3) 和 式 〈10-4) 计算 每 个 抗体 的 亲 和 度 aff, (i) K 
抗体 与 抗体 之 间 的 排斥 力 off, (i, j) o 

步骤 4: 克隆 抗体 。 按 照 每 个 抗体 的 亲 和 度 对 抗体 进行 克隆 ， 克 隆 数目 knum (i) 由 
R (10-5) ME. 

步骤 5: 变异 。 按 照 式 (10-6) 对 所 有 抗体 进行 变异 。 

步骤 6: 选 优 。 计算 变异 后 所 有 抗体 的 aff.(i) 和 of (i,7)， 抛 弃 off.(i, 站 <Th 中 的 一 个 
抗体 ， 并 按 af, (进行 排序 ， 选 出 前 面 的 个 抗体 更 新 记忆 库 。 返 回 步骤 3， 直 到 满足 
终止 判 据 为 止 。 

步骤 7: 估计 。 用 记忆 库 中 的 个 抗体 作为 新 一 代 的 粒子 ， 计 算 Xk+1 时刻 的 状态 估 
计 ， 并 进行 重 采 样 。 

步骤 8: 令 k=k+1， 返 回 步骤 2。 

由 算法 的 实现 过 程 看 ， 在 每 一 时 刻 k， 可 能 涉及 抗体 群 的 多 次 迭代 和 更 新 ， 从 而 使 
粒子 得 以 优选 ， 提 高 了 对 系统 状态 的 估计 能 力 。 


10.3 人工 鱼 群 粒子 滤波 


10.3.1 人 工 鱼 群 算法 原理 


人 工 鱼 群 算法 (AFSA) “ “是 一 种 通过 模拟 鱼 群 在 自然 环境 中 的 生态 行为 得 到 的 
随机 搜索 优化 算法 ， 这 种 算法 不 需要 问题 的 严格 机 理 模 型 ， 从 而 使 其 具有 广泛 的 应 用 范 
围 。 人 工 鱼 是 真实 鱼 个 体 的 一 个 虚拟 实体 ， 这 个 实体 封装 了 人 工 鱼 自身 数据 信息 和 一 系 
列 的 行为 。 人 工 鱼 所 在 的 环境 主要 是 问题 的 解 空 间 和 其 他 人 工 鱼 的 状态 ， 它 在 下 一 时 刻 
的 行为 取决 于 目前 自身 的 状态 和 目前 环境 的 状态 (包括 问题 当前 解 的 优 和 劣 和 其 他 同伴 的 
状态 )， 并 通过 自身 的 活动 来 影响 环境 ， 进 而 影响 其 他 同伴 的 活动 。 

AFSA 是 一 种 集群 智能 算法 。 一 个 集群 通常 定义 为 一 群 自治 体 的 集合 ， 它 们 通过 相 
互 直 接 或 者 间接 的 通信 及 全 体 的 活动 来 解决 一 些 分 布 式 难题 。 这 里 ， 自 治 体 是 指 在 一 个 
环境 中 具备 自身 活动 能 力 的 实体 ， 其 自身 力求 简单 ， 通 常 不 必 具 有 高 级 智能 ， 但 它们 的 
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集群 活动 所 表现 出 来 的 则 是 一 种 高 级 智能 才能 达到 的 活动 ， 称 这 种 活动 为 集群 智能 。 

假设 虚拟 人 工 鱼 实体 的 当前 状态 为 x Visual 为 其 视野 范围 ， 状 态 x, 为 其 在 某 时 刻 
视点 所 在 的 位 置 。 如 果 该 位 置 的 状态 优 于 当前 状态 ， 则 考虑 向 该 位 置 方向 前 进一步 ， 即 
到 达 状 态 xus 如 果 状 态 x, 不比 当 前 状态 更 优 ， 则 继续 巡视 视野 内 的 其 他 位 置 。 巡 视 的 
次 数 越 多 , 则 对 视野 内 的 状态 了 解 更 全 面 ， 从 而 对 周围 的 环境 有 一 个 全 方位 立体 的 认 知 ， 
这 有 助 于 做 出 相应 的 判断 和 决策 。 当 然 , 对 于 状态 多 或 无 限 状 态 的 环境 也 不 必 全 部 遍历 ， 
允许 一 定 的 不 确定 性 对 于 摆脱 局 部 最 优 、 寻 找 全 局 最 优 是 有 帮助 的 。 假 设 状态 
X= (AX X,-(X4,X,257,x,,)» VOTER UA 


X, , = x, + Visual -u 


5-3 ° d 

XP: u-U[0,1] 为 0—1 的 随机 数 ，StepSize 为 移动 步 长 。 由 于 环境 中 同伴 的 数目 是 有 限 
的 ， 因 此 ， 在 视野 中 感知 同伴 的 状态 ， 并 相应 地 调整 自身 状态 的 方法 和 上 式 类 似 。 

在 AFSA 中 ， 一 个 重要 的 概念 是 鱼 群 的 “行为 模式 ” 它 表 征 了 鱼 类 各 种 活动 的 表 
现形 式 ， 通 常 包括 以 下 几 种 。 

(OD MAITA (Prey behaviour)。 这 是 生物 的 一 种 最 基本 的 行为 ， 也 就 是 趋向 食物 
的 一 种 活动 。 一 般 情况 下 ， 鱼 在 水 中 随机 地 自由 游 动 ， 当 发 现 食物 时 ， 则 会 向 食物 逐渐 
增多 的 方向 快速 游 去 ， 可 以 认为 它 是 通过 视觉 或 味觉 来 感知 水 中 的 食物 量 或 浓度 来 选择 
趋向 的 。 因 此 ， 以 上 所 述 的 视觉 概念 可 以 应 用 于 该 行为 。 

(2) RITA (Swarm behaviour)。 这 是 鱼 类 较 常 见 的 一 种 现象 ， 大 量 或 少量 的 鱼 
都 能 聚集 成 群 ， 这 是 它们 在 进化 过 程 中 形成 的 一 种 生存 方式 。 鱼 在 游 动 过 程 中 为 了 保证 
自身 的 生存 和 躲避 危害 会 自然 地 聚集 成 群 ， 鱼 聚 群 时 所 道 守 的 规则 有 三 条 : DARA. 
尽量 避免 与 临近 伙伴 过 于 拥挤 。@ 对 准 规 则 。 尽 量 与 临近 伙伴 的 平均 方向 一 致 。@ 内 育 
规则 。 尽 量 朝 临近 伙伴 的 中 心 移动 。 

(3) 追尾 行为 (Follow behaviour)。 当 鱼 群 中 的 一 条 或 几 条 鱼 发 现 食物 时 ， 其 临近 
的 伙伴 会 尾随 其 快速 到 达 食物 点 。 

(4) 随机 行为 。 鱼 在 水 中 悠闲 地 自由 游 动 ， 基 本 上 是 随机 的 ， 其 目的 是 为 了 在 更 大 
范围 内 寻觅 食物 或 伙伴 。 

每 条 人 工 鱼 通过 对 环境 的 感知 ， 在 每 次 移动 中 经 过 尝试 后 ， 执 行 其 中 的 一 种 行为 。 
人 工 鱼 群 算法 就 是 利用 这 几 种 典型 行为 从 构造 单条 鱼 底层 行为 做 起 ， 通 过 鱼 群 中 每 一 
个 体 局 部 寻 优 达 到 全 局 最 优化 的 目的 。 算 法 的 进行 就 是 人 工 鱼 个 体 的 自 活动 过 程 ， 整 
个 过 程 主 要 包括 觅 食 、 聚 群 以 及 追尾 等 几 种 行为 ， 最 优 解 将 在 该 过 程 中 突现 出 来 。 其 
中 葛 食 行为 是 人 工 鱼 根据 当前 自身 的 适应 值 随 机 游 动 的 行为 ， 是 一 种 个 体 极 值 寻 优 过 
程 ， 属 于 自学 习 的 过 程 ， 而 聚 群 和 追尾 行为 则 是 人 工 鱼 与 周围 环境 交互 过 程 。 这 两 种 
过 程 是 在 保证 不 与 伙伴 过 于 拥挤 ， 且 与 临近 伙伴 的 平均 移动 方向 一 致 的 情况 下 向 群体 
极 值 (中 心 ) 移动 。 

如 何 利用 简便 、 有 效 的 方式 来 构造 并 实现 页 食 、 聚 群 、 尾 追 等 行为 是 AFSA 实施 的 
关键 ， 下 面 分 别 进行 介绍 。 
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.StepSize - u 


|l. KRITA 

假设 人 工 鱼 个 体 的 状态 可 用 矢量 表示 为 x= (x ,x,,…,x,)， 其 中 ，x(i=1,…,n) 为 欲 
寻 优 的 变量 ， 人工 鱼 当前 所 在 位 置 的 食物 浓度 表示 为 y= f(x), EP, y 为 目标 函数 值 ; 
人 工 鱼 个 体 之 间 的 距离 表示 为 d = |x, -x,|; v 表示 人工 鱼 的 感知 距离 。 

设 人 工 鱼 当前 状态 为 x, , 在 其 感知 范围 内 ( d, < v ) 随 机 选择 一 个 状态 x , 若 y < yj 
则 向 该 方向 前 进一步 ， 若 不 满足 ， 则 随机 移动 一 步 ， 即 

X*; — Xi 

|x, -x| 
else x, I = x, +r-s 
AP: s 表示 人 工 鱼 移动 的 最 大 步 长 ，r~U[0,1] 为 维 数 与 x 相同 的 随机 矢量 ;r~Z[0,]1] 为 
介 于 0 一 1 的 一 个 随机 数 。 

2. RETA 

人 工 鱼 在 游 动 过 程 中 自然 聚集 成 群 ， 当 在 其 感知 范围 内 〈d, «vo 探索 其 他 集群 伙 


if y, « yj, Xie 7X; * 1757 


N 
HME N, bab x= LY e SE UN, > 5 ， 表 明 伙伴 中 心 有 很 多 食物 并 且 
f jn 


不 太 拥 挤 ， 则 向 伙伴 中 心 移动 ， 即 


Ye — x, 


X. 


x. -x 








>J, X, = X; +r °S - 


否则 ， 执 行 更 食 行为 。 式 中 ，5 为 拥挤 度 因子 。 
3. 追尾 行为 
当 一 条 鱼 发 现 食物 后 ， 其 他 人 工 鱼 会 尾随 它 向 食物 游 去 。 搜 索 当前 领域 中 食物 浓度 最 
大 的 人 工 鱼 所 在 位 置 yaw ， 当 Imo ay 时 ， 则 向 食物 浓度 最 大 的 人 工 鱼 处 前 进一步 ， 即 
f 


X, = XQ + °S 


|x, 7 x] 
FU, BUTMETTA. 

4. 行为 选择 

根据 所 要 解决 的 问题 性 质 ， 对 人 工 鱼 当前 所 处 的 环境 进行 评价 ， 从 上 述 各 行为 中 选 
择 一 种 合适 的 行为 。 常 用 的 方法 有 两 种 。 

(D 先进 行 追 尾行 为 ， 如 果 没 有 进步 则 进行 聚 群 行为 ， 如 果 依 然 没 有 进步 则 进行 
觅 食 行为 。 也 就 是 选择 较 优 的 行为 前 进 ， 即 任 选 一 种 行为 ， 只 要 能 向 更 优 的 方向 前 进 
即 可 。 

(2) 试探 执行 各 种 行为 ， 选 择 各 行为 中 使 得 向 最 优 方向 前 进 最 快 的 行为 ， 即 模拟 聚 
群 、 追 尾 等 行为 ， 然 后 选择 能 导致 该 行动 后 使 状态 更 优 的 动作 ， 缺 省 的 行为 方式 是 觅 食 
行为 。 也 就 是 选择 各 行为 中 使 得 人 工 鱼 的 下 一 个 状态 最 优 的 行为 ， 如 果 没 有 能 使 人 工 鱼 
的 下 一 状态 优 于 当前 状态 的 行为 ， 则 采取 随机 行为 。 对 于 此 种 方法 ， 同 样 的 迭代 步 数 寻 
优 效果 会 好 一 些 ， 但 计算 量 也 会 加 大 。 
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人 工 鱼 群 算法 通过 这 些 行 为 的 选择 形成 了 一 种 高 效 的 寻 优 策略 ， 最 终 ， 人 工 鱼 将 集 
结 在 目标 函数 的 极 值 区 域 。AFSA 的 基本 流程 如 图 10.3 所 示 。 


人 工 鱼 群 初始 化 
(个 体 数 为 N) 






第 条 人 工 鱼 尝试 追尾 行为 


图 10.3 人 工 和 鱼 群 算法 流程 图 


1032 ”人 工 鱼 群 粒子 滤波 算法 


若 将 人 工 鱼 的 个 体 与 粒子 对 应 ， 将 系统 的 观测 方程 与 目标 函数 对 应 ， 在 传统 的 粒子 
滤波 算法 中 融入 AFSA 的 聚 群 行为 和 驶 食 行为 所 对 应 的 粒子 状态 更 新 公式 ， 即 可 得 到 人 
工 鱼 群 粒子 滤波 (AFSPF) 算法 ， 具 体 步骤 可 概括 如 下 : 

Alg (10.4): Artificial Fish Swarm Particle Filtering 

步骤 1: 初始 化 

在 上 =0 时 刻 ， 从 重要 性 函数 中 采样 得 到 N 个 粒子 ， 即 {xP , 1N o 

步骤 2: 粒子 时 间 更 新 

通过 采样 函数 q(xf | x) 2,) 产生 粒子 ， 若 采样 函数 为 先 验 分 布 ， 即 

q(x | x) > Z) = p(xP | Xp) 
Jul x? = Gaara aea )-p(xp |x) ° 


步骤 3: 重要 权 值 计算 
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uP - ups |Ë) 
令 目标 函数 >= 忆 (zx - £a) , v,-N(0,R,), MI 


y=- J Ks 89 O| 
Jm)": det (R,) 2 
(1) 更 食 行为 。 当 y,< yy 时， 通过 与 目标 函数 进行 比较 ， 不 断 更 新 粒子 的 状态 使 其 
向 真实 值 靠近 。 
FA: 
[NIE T 17 xz: +| 


RP: $) ADU RO S m 次 (m=1,2,…,M ) 迭代 后 的 结果 。 ERI, $09. m £D eris. 
(2) 聚 群 行为 。 设 ¿a EE v 内 的 其 他 预测 量 测 值 集合 为 4 AARE A 的 势 ， 


Slim) c1) 
X = Xii Tre 








1 1 $i0 ü ! Va AA a 
Wut 20) = TA] a... EGP y» W = +r- "Et s] TEES 
jzi Ik-1 klk-l 

否则 转 婉 食 行为 。 


算法 的 终止 可 以 通过 限制 最 大 迭代 次 数 M 或 者 以 相 邻 两 次 迭代 中 目标 函数 的 偏差 
小 于 某 一 预 设 的 门限 为 条 件 。 
步骤 4: 权 值 更 新 


根据 wp = ul) p(z, | x) 更 新 权 值 ， 并 通过 名 - uy up 对 取出 的 Ni 个 粒子 的 权 
值 进行 归 一 化 。 

步骤 5， 估计 输出 

状态 估计 :名 = -2 0 


方差 估计 :Pp = -> WPL -L -2 


步骤 6: 判断 是 否 结束 
若是 则 退出 本 算法 ， 若 否则 返回 步骤 2。 


10.4 7k 章 小 结 


粒子 滤波 算法 的 性 能 取决 于 对 状态 后 验 分 布 的 近似 精度 ， 而 近似 精度 又 跟 粒 子 的 数 
E. ME (有 效 性 、 多 样 性 ) 等 因素 密切 相关 。 因 此 ， 为 了 提高 粒子 滤波 算法 的 估计 性 
能 ， 一 条 重要 的 途径 就 是 提高 粒子 的 产生 质量 ， 避 免 粒子 退化 和 贫 化 。 在 第 8 章 中 ， 主 
要 阐述 通过 对 不 同 重要 性 采样 函数 的 近似 来 达到 提高 粒子 产生 质量 的 目的 。 本 章 则 从 另 
一 个 角度 ， 即 通过 将 人 工 智 能 优化 选择 与 粒子 滤波 相 结合 的 手段 ， 重 点 介绍 了 遗传 粒子 
滤波 、 人 工 免疫 粒子 滤波 和 人 工 鱼 群 粒子 滤波 的 算法 原理 和 实现 方法 。 这 类 滤波 算法 的 
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相似 之 处 是 利用 了 人 工 智 能 的 优化 策略 ， 在 粒子 产生 和 更 新 的 过 程 中 增加 了 优选 环节 ， 
保持 了 粒子 的 多 样 性 ， 同 时 使 其 往 高 似 然 区 域 移动 和 聚集 。 

应 该 指出 的 是 ， 作 为 提高 粒子 滤波 性 能 的 一 条 重要 途径 ， 基 于 最 优化 理论 的 粒子 优 
选 方法 越 来 越 受 到 重视 。 理 论 上 ， 除 了 前 文 涉及 的 几 种 重要 方法 外 ， 人 工 智 能 与 最 优化 
理论 中 的 其 他 一 些 方法 都 具备 与 粒子 滤波 结合 的 潜力 ， 且 各 种 方法 的 组 合 优化 可 望 进 一 
步 提 高 粒子 滤波 的 整体 估计 性 能 ， 关 于 这 一 点 ， 可 参阅 文献 [66] 一 文献 [68]。 
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本 


在 贝 叶 斯 滤波 的 统一 框架 下 ， 本 书 第 二 部 分 和 第 三 部 分 从 不 同 的 近似 角度 ， 阐 述 了 
一 般 条 件 下 的 非 线性 / 非 高 斯 滤波 问题 ， 主 要 基于 两 点 基本 的 假设 ; 中 系统 模型 只 有 单一 
模式 ， 即 虽然 系统 状态 是 动态 变化 的 ， 但 在 所 讨论 的 时 间 范 围 内 其 演化 规则 是 唯一 的 ; 
@ 基 于 各 种 方法 的 系统 状态 估计 是 在 单传 感 器 观测 条 件 下 得 到 的 ， 即 同一 时 刻 尽管 因为 
杂 波 或 噪声 的 影响 存在 多 个 量 测 〈( 包 括 虚 警 )， 但 这 些 观测 信息 源 于 单一 传感器 。 

但 是 ,现实 世界 中 面临 的 问题 往往 比 以 上 假设 条 件 更 为 复杂 , 如 对 于 目标 跟踪 而 言 ， 
目标 的 机 动 会 导致 其 运动 模式 〈 或 运动 状态 演化 规则 ) 的 变化 ， 而 无 论 是 对 于 民用 还 是 
军事 目标 ， 出 于 特定 的 目的 需要 ， 执 行 相应 的 机 动 动作 是 非常 普遍 的 ， 另 外， 为 了 提高 
对 目标 的 跟踪 能 力 ， 采 用 单 / 多 平台 多 传感器 协同 处 理 也 是 常用 的 手段 。 在 这 种 条 件 下 ， 
前 述 的 滤波 理论 和 方法 又 面临 新 的 挑战 。 

本 部 分 主要 以 第 二 部 分 和 第 三 部 分 的 理论 和 方法 为 基础 , 进一步 分 析 两 种 扩展 条 件 ， 
即 多 传感器 观测 和 混合 系统 模型 条 件 下 的 非 线性 滤波 及 其 应 用 问题 。 第 11 章 主要 针对 集 
中 式 融 合 和 分 布 式 融 合 两 种 结构 ,介绍 多 传感器 融合 估计 方法 ; 第 12 章 则 主要 针对 混合 
系统 的 估计 问题 ， 重 点 介绍 多 模 滤波 算法 。 每 章 最 后 给 出 了 基于 解析 高 斯 滤波 和 近似 采 
样 滤波 的 目标 跟踪 应 用 实例 。 


第 11 章 多 传感器 融合 滤波 


目前 ， 在 军事 和 民用 领域 中 ， 目 标的 监视 、 检 测 、 跟 踪 以 及 识别 处 理 等 很 多 都 是 
基于 多 传感器 的 量 测 ， 利 用 信息 融合 (Information Fusion) 技术 来 完成 的 。 所 谓 信息 
融合 0 就 是 将 来 自 多 个 传感器 或 多 源 的 信息 进行 综合 处 理 ， 用 以 得 出 更 为 准确 、 可 靠 
的 结论 。 

与 单传 感 器 系统 相 比 较 ， 多 传感器 信息 融合 系统 尽管 增加 了 系统 的 复杂 性 ， 但 具有 
单传 感 器 系统 所 不 具备 的 诸多 优势 ， 如 可 有 效 扩展 信息 的 时 间 、 空 间 和 频率 〈 谱 段 ) 覆 
盖 范 围 ， 增 加 量 测 空间 维 数 ， 降 低 信 息 的 模糊 度 ， 提 高 处 理 结果 可 信和 度 等 ， 此 外 ， 特 定 
的 融合 结构 还 可 增强 系统 的 生存 能 力 ， 改 善 系统 的 可 靠 性 。 

融合 滤波 的 一 个 重要 目的 就 是 综合 利用 多 个 传感器 的 量 测 信 息 ， 来 提高 系统 状态 估 
计 精 度 。 对 于 一 些 特 殊 的 应 用 ， 如 只 测 角 跟踪 中 ， 在 单传 感 器 配置 条 件 下 ， 所 能 够 获取 
的 目标 信息 将 会 非常 有 限 ， 加 上 受 可 观测 性 条 件 的 限制 ， 严 重 限制 了 被 动 系统 对 目标 的 
探测 与 跟踪 能 力 。 利 用 多 传感器 信息 进行 融合 处 理 可 望 获得 较 大 的 能 力 提升 ， 不 仅 可 以 
降低 对 观测 器 的 机 动 性 要 求 ， 而 且 能 够 得 到 更 加 精确 的 目标 航 迹 估计 。 

本 章 首 先 给 出 了 不 同 融合 结构 中 的 贝 叶 斯 滤波 统一 框架 ， 进 而 讨论 线性 、 高 斯 条 件 
下 的 最 优 融合 算法 ， 并 将 其 推广 到 非 线性 系统 中 ， 最 后 通过 具体 的 实例 对 融合 算法 进行 
了 仿真 验证 。 


11.1 融合 估计 系统 结构 


采用 多 传感器 观测 进行 融合 估计 处 理 ， 有 着 多 种 实现 途径 ， 按 照 融 合 结构 的 不 同 ， 
可 以 分 为 集中 式 融 合 (Centralized Fusion)、 分 布 式 (Distributed Fusion) 融合 以 及 混合 式 
融合 (Hybrid Fusion) 融合 结构 。 

集中 式 融 合 结构 如 图 11.1 所 示 ， 在 这 种 处 理 结构 中 ， 融 合 中 心 可 以 利用 所 有 传感器 
的 原始 量 测 数据 〈 故 也 称 为 量 测 融合 (Measurement Fusion))， 没 有 任何 信息 的 损失 ， 
而 理论 上 这 种 融合 结果 是 最 优 的 。 但 是 这 种 结构 同样 存在 一 些 自身 的 问题 ， 主 要 体现 在 
以 下 几 个 方面 ; 





1.1 集中 式 结构 
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(1) 原始 量 测 信息 需 直 接 传送 至 融合 中 心 ， 对 通信 带宽 和 实时 传输 能 力 要 求 较 高 。 

(2) 融合 中 心 要 具备 强大 的 计算 能 力 ， 以 满足 大 数据 量 处 理 要 求 。 

(3) 在 异 质 传感器 条 件 下 ， 传 感 器 原始 观测 数据 形式 不 同 ， 难 以 直接 进行 融合 。 

(4) 传感器 原始 观测 数据 的 配 准 难 度 较 大 ， 无 论 是 时 间 配 准 还 是 空间 配 准 。 

除 此 之 外 ， 由 于 融合 处 理 过 程 过 分 依赖 于 融合 中 心 ， 使 得 集中 式 融 合 系统 的 生存 能 
力也 下 降 。 

分 布 式 融 合 结构 如 图 11.2 所 示 ， 在 这 种 结构 中 ， 每 个 局 部 传感器 (节点 ) 都 有 上 自己 
的 处 理 器 ， 经 自身 处 理 后 将 中 间 结 果 送 到 融合 中 心 。 由 于 各 传感器 都 有 自己 的 局 部 处 理 
器 ， 能 够 得 到 各 自 的 滤波 估计 结果 ， 因 此 这 种 结构 通常 也 称 为 航 迹 融 合 (Track Fusion), 
融合 中 心得 到 全 局 的 估计 结果 。 与 集中 式 融合 相 比 ， 分 布 式 融 合 得 到 的 结果 通常 是 次 优 
的 ; 但 这 种 融合 方式 因 具 有 对 通信 要 求 低 、 系 统 生命 力 强 、 工 程 实现 相对 容易 等 特点 而 
广 受 关注 。 

典型 的 混合 式 融 合 结构 如 图 11.3 所 示 ， 顾名思义 ， 这 种 融合 结构 是 集中 式 和 分 布 式 
融合 的 混合 体 ， 融 合 中 心得 到 的 部 分 为 原始 量 测 数据 ， 部 分 为 经 由 某 些 局 部 节点 处 理 过 
的 中 间 数 据 。 由 于 这 种 结构 兼 具 集 中 式 融 合 和 分 布 式 融合 两 种 结构 的 特点 ， 因 而 在 优 缺 
点 方面 也 是 两 者 的 折 中 ; 但 是 ， 这 种 结构 方式 为 处 理 多 平台 、 多 个 同类 /异类 传感器 的 融 
合 问题 提供 了 方便 。 





图 11.2 分布 式 结构 图 11.3 ”混合 式 结构 


鉴于 集中 式 融 合 与 分 布 式 融合 在 现实 世界 中 的 普遍 性 和 应 用 的 广泛 性 ， 接 下 来 主要 
针对 这 两 种 结构 进行 详细 介绍 和 分 析 。 


11.2 集中 式 融 合 滤波 算法 


考虑 以 下 包含 多 传感器 量 测 的 非 线 性 系统 模型 . 
X, = f(x, ,w,) (11-0) 
y,-h(x,,v) s-12,-.,S (11-2) 
AP: 以 为 第 个 传感器 获取 的 量 测 ， 磊 () 为 该 传感器 的 观测 方程 ，5 为 传感器 个 数 。 
若 用 yi; 表示 到 大 时 刻 为 止 所 有 传感器 获取 的 累积 量 测 ， 即 yr =y 
y =OS ， 则 根据 贝 叶 斯 原理 ， xx ERS pev) 可 通过 以 下 两 步 递 推 得 到 。 
预测 : 假定 上 -1 时刻 状 态 的 后 验 PDF 为 p(x, |)， 则 其 一 步 前 向 预测 的 PDF 
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可 表示 为 
p(x, | P) - fo |x, P(X | Fiki Xx, , (11-3) 
式 中 ， 隐 含 假定 了 p(x, | XP Xaia) P | xi) 称 为 转移 密度 函数 ， 用 于 描 
述 式 〈11-1) 中 的 一 阶 马 尔 可 夫 过 程 。 
滤波 : 在 获得 p(x,| yz.) 的 基础 上 ， 综 合 新 近 的 量 测 yrs ， 可 通过 如 下 的 贝 叶 斯 公 
式 计 算 上 时 刻 状 态 的 后 验 PDF: 
1:S 1:S ks 
px, Ll) = ps, Ll yis) = £O Paru OPO 1-0 PTT , za [ya Hals 
< p(p¥ |x, )p(x, | Yi) 
进一步 假定 各 传感器 观测 独立 ， 于 是 有 
p(x, | yz) < POVE |x, )p(x, | ia) POI E Vi XP | Ya) 


5 
=p(x, | y| | P(Y: |x,) 
s=1 


式 〈11-5) 即 为 采用 集中 式 融合 结构 得 到 的 贝 叶 斯 估计 表达 式 ， 由 此 可 以 看 出 ， 对 
于 一 般 的 非 线 性 系统 , 上 式 很 难得 到 解析 表达 式 。 而 在 线性 、 高 斯 条 件 下 , 即 若 式 (11-1)、 
A (11-2) 可 以 写成 


(11-5) 


x, = Fx, ,*w, (11-6) 

y, = H'x +w s=1,2,.…,S (11-7) 
式 中 : w, 为 零 均值 高 斯 过 程 噪声 且 cov[w, ]=Q, : v; 为 零 均 值 高 斯 观测 噪声 且 cov [vz ]=R; 。 
则 可 以 证 明 〈 见 附录 F)， 上 述 递 推 式 存在 最 优 解 ， 且 解 的 形式 为 


Xua = FX, , (11-8) 
By, = FP, ,F! + @, (11-9) 
X, = Xa + K,[y, - Hx] (11-10) 
P, -Pa +H" R;'H (11-11) 
K, = P,H" Rọ’ (11-12) 
式 中 
»7lo 6 0: ] (11-13) 
R; -diag[(R; ) ' (RE) --- (RE) |] (11-14) 
H=[(H,)' (Hi) =- (H$) T (11-15) 
将 式 〈11-13) 一 式 (11-15) 分 别 代 入 式 〈11-10) MA (11-11), f8 
X, = Xie OX O -H'£44) (11-16) 
B/-B3 S3 Qi Hz (anm 


s] 


需要 指出 的 是 ， 由 式 〈11-5) 的 表达 式 可 知 ， 上 述 融合 算法 主要 是 针对 在 同一 时 刻 
各 传感器 的 量 测 噪声 之 间 互 不 相关 的 情形 。 然 而 在 许多 实际 的 应 用 条 件 下 ， 这 一 假设 条 
件 不 总 是 满足 的 (如 在 共同 的 噪声 环境 中 对 目标 运动 状态 进行 观测 ， 其 传感器 的 观测 噪 
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声 之 间 存 在 一 定 的 相关 性 ， 又 如 即使 某 些 观测 误差 在 原始 坐标 系 中 是 不 相关 的 ， 但 经 非 
线性 坐标 变换 后 由 于 误差 依赖 于 状态 可 能 变 得 相关 )。 对 于 观测 噪声 相关 的 情形 ， 相 应 的 
融合 算法 可 参阅 文献 [6] 和 文献 [7]。 
另外 ， 对 于 一 般 的 非 线 性 系统 ， 式 (11-5) 的 闭 式 解 并 不 存在 ， 此 时 需要 求 次 优 解 。 
一 种 可 行 的 方法 是 用 扩展 卡尔 曼 滤 波 代替 卡尔 曼 滤波 算法 ， 此 时 分 别 用 
pI ,pW 
âx Ox dnd 


fF K H° , 从 而 得 到 基于 扩展 卡尔 曼 滤波 的 集中 式 融 合 滤波 算法 , 其 流程 可 概括 如 下 。 
Alg (11.1): EKF based centralized fusion filtering 


^ 
XXE L4 


Xu = fu) (11-18) 
Pa = F,P, F; + @, (11-19) 
$, = Xii + K,[y, - h(k- )] (11-20) 
P;'=P;; +H" R;'H (11-21) 
K, = P,H" R; (11-22) 
AP: y, R, 的 定义 与 式 (11-13) AA 01-14) 中 的 相同 ， 且 
H-(H;) (H;) (He) (11-23) 
hKOR OD (60) 05 OT (11-24) 


对 于 本 书 中 第 二 部 分 提 到 的 其 他 类 型 的 高 斯 近似 非 线性 滤波 方法 (如 UKF, CDKF. 
GHKF 及 CKF 等 )， 由 于 不 存在 显 式 的 FF 和 右 ,， 不 能 作 类 似 的 处 理 。 但 由 于 式 (11-11) 


和 式 (11-12) 可 改写 成 


P=P - K,P, , Kr (11-25) 
K, =P ,, P, (11-26) 
因此 只 要 通过 其 他 方式 求 得 已 ,， 和 已 ，， 同 样 可 以 实现 相应 的 融合 滤波 处 理 , 在 此 


ABE. 
上 述 介绍 的 是 在 卡尔 曼 滤 波 框架 下 的 融合 算法 ， 在 粒子 滤波 框架 下 ， 状 态 的 联合 后 
验 分 布 Pal DA) C p(X Hia) A POGa | yi) 的 边缘 化 形式 ) 被 表示 成 一 组 随机 采样 
OY. 的 形式 ， 且 各 采样 样本 的 重要 性 权 值 为 
(i) pt Er ) 
UT Ly) 
式 中 : pa Ly) 根据 贝 叶 斯 定理 可 进一步 写成 
por lx, )p(x, |x, 1) LS 
POP waa) Plisi Era) (11-28) 
< pO bx, )pGo le,  p(m, la 
若 将 重要 权 定 义 为 满足 如 下 的 因 式 分 解 形式 ， 即 
qq, | ys )s q(x, ik- Vik Gy | hea) (11-29) 


(11-27) 


PG, | ys F 


则 权 值 的 递 推 式 满足 
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i i pO D pt xe) 
同样 ， 若 假设 各 传感器 之 间 的 观测 噪声 相互 独立 ， 则 式 〈11-30) 等 价 于 
s 

po p) [pyri ) 

S=1 

qx? xis s Ji) 

式 中 ， 与 单传 感 器 观测 条 件 下 的 重要 性 权 值 计算 类 似 ， 多 传感器 下 的 重要 性 权 值 计算 同 
样 依赖 于 重要 性 采样 函数 q(x, n Ju ) 选 择 , 当 qx, bns, Ju )=p(x,|x, ,) 时 , 式 (11-31) 
可 进一步 简化 为 


wP oc wp), (11-31) 


S 
wP c u, [560:bx2) (11-32) 


s=1 


对 于 其 他 的 选择 方式 ， 同 样 可 以 假定 最 优 重要 权 满 足 


S 
q(x, Xix- Jik )=q (xX, bx a JE )=[ I q(xXi bea ai) 
es (11-33) 


S 
~ [N iR) 
s=l 


对 于 其 他 更 一 般 的 情况 ， 其 计算 过 程 要 比 上 述 复杂 ， 如 对 于 观测 噪声 相关 的 情形 ， 
可 采用 主 分 量 (PCA) 等 去 相关 的 手段 将 其 变换 成 非 相 关 噪声 ， 进 而 采用 相应 的 方法 
来 处 理 ， 具 体 过 程 可 参阅 文献 [9]。 


113 ”分布 式 融 合 滤波 算法 


在 如 图 11.2 所 示 的 分 布 式 融 合 结构 中 ， 对 来 自 不 同 传感器 航 迹 〈 传 播 到 同一 时 刻 ) 
的 状态 估计 进行 关联 和 融合 以 得 到 系统 航 迹 的 状态 估计 。 在 这 一 过 程 中 ， 不 利用 系统 航 
迹 以 前 的 状态 估计 。 因 此 ， 对 于 每 一 个 传感器 ， 其 从 量 测 到 状态 的 估计 仍 可 通过 第 1 章 
所 介绍 的 贝 叶 斯 公式 进行 计算 ， 即 对 第 s 个 传感器 有 
p(x, | Vix) = [p(x |x, ,)p(x, | yr a Xx, (11-34) 
p(x, | yi) = p(x, | Yi a) < POX | x, )p(x, | yia) (11-35) 
根据 特定 的 非 线性 系统 状态 ， 对 上 述 两 式 进行 求解 可 得 到 各 种 次 优 解 ， 即 得 到 各 个 
传感器 基于 量 测 值 的 非 线性 局 部 估计 〈 可 通过 第 二 部 分 和 第 三 部 分 介绍 的 方法 得 到 ) 。 
分 布 式 融 合 的 目的 是 根据 这 些 局 部 估计 得 到 全 局 估计 ， 根 据 不 同 的 条 件 ， 可 得 到 不 同 的 
融合 策略 与 规则 外 ， 其 中 有 些 融合 方法 是 最 优 的 、 有 的 是 次 优 的 。 这 里 仅 对 几 种 最 有 代 
表 性 的 融合 算法 进行 详细 介绍 。 
由 系统 模型 式 (11-6) 和 式 〈11-7) 可 知 ， 对 于 分 布 式 融 合 结构 ， 每 个 局 部 传感器 s 
的 最 优 估 计 结 果 可 以 用 信息 滤波 的 形式 表示 为 
i = Sipa +R (EY (RE'O -H ipa) (11-36) 
(Pi) 75, ,) +(H;) (RO H; (11-37) 
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在 分 布 式 融 合 结构 中 ， 中 心 融合 器 将 不 直接 使 用 量 测 来 获取 全 局 估计 ， 而 仅仅 联合 
各 传感器 的 局 部 估计 来 得 到 全 局 估计 。 为 此 首先 将 式 〈11-36) AA (11-37) 左右 两 边 
对 应 相 乘 ， 得 
BP) Pua) HH) (R;) H] 
Tip- + P, (H SRY (y; -H “Kt-1)] 
=[(Pi_) +(H;) (Ri) H; Kip- (11-38) 
+P) PI(H’) (R; Ot - H" X3) 
num ) Xii HH; ) (R, 7X 
于 是 有 
(Hi) (Ri) y 7G) X, - (Pik) Xia (11-39) 
接 下 来 ， 将 用 上 式 来 消除 融合 中 心 更 新 方程 中 的 量 测 信 息 。 在 此 之 前 首先 根据 集中 
式 融 合 的 结果 


3, = Sea +P yar yaty'o! -H'£qa) (11-40) 

Bj -Bj S D) (Q) H,; (11-41) 
XX (11-40) 和 式 ( 11.41) 采用 与 前 述 类 似 的 等 号 两 边 相 乘 的 方法 得 

E & = Bz aa «3r RD” yi (11-42) 
将 式 (11-39) RAR (11-42), f ü 

p'a = Fi ta UT E -Pha Hya (11-43) 

同 理 ， 将 式 11-37) 变形 ， 得 | 

(H,) (RO H;=(P;) - (Pika) (11-44) 
将 式 〈11-44) RAR 〈11-41)， 得 

pi =P OD - Pha”) (11-45) 
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综 上 , A (11-43) MAA (11-45) 即 为 采用 分 布 式 结构 得 到 的 融合 结果 ， 由 推导 过 程 
可 以 看 出 ， 该 融合 算法 是 通过 具有 全 局 最 优 的 集中 式 融 合算 法 通过 和 矩阵 变换 得 到 的 ， 因 
而 也 是 全 局 最 优 的 。 

需要 指出 的 是 , 上 述 融 合算 法 中 , 融合 中 心 不 仅 需要 各 传感器 的 滤波 估计 P Ea > 
同时 还 需要 各 自 的 预测 估计 {总 ,_1,Pi_1}s-1， 当然 这 些 信息 对 一 般 的 序 贯 滤波 算法 而 言 都 
能 得 到 。 从 式 (11-430 和 式 (11-45) 可 以 看 出 ， 这 种 融合 算法 不 依赖 于 单个 传感器 具 
体 的 滤波 算法 形式 ， 为 各 种 非 线 性 滤波 算法 的 应 用 提供 了 方便 。 

但 同样 需要 强调 的 是 ， 上 述 结 果 是 在 线性 、 高 斯 条 件 下 导出 的 ， 对 于 一 般 的 非 线 性 
系统 , 上 述 融合 算法 在 具体 应 用 时 需 根据 不 同 的 滤波 方法 进行 修正 。 特别 地 , 当 采 用 EKF 
FEN, Hi 对 应 于 各 节点 传感器 观测 方程 的 偏 导数 。 而 对 于 UKF, CDKF 或 CKF 等 其 
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他 滤波 算法 ， 则 需要 进行 特殊 的 处 理 才能 实现 融合 。 下 面 以 文献 [10] 中 的 不 敏 信息 滤波 


算法 为 例 ， 介 绍 基于 UKF 的 融合 过 程 。 
对 于 下 式 所 描述 的 系统 方程 
X, = J (XW) 
y, = h(x,)*v, 
若 定义 Fisher fci BABEE J =P, ! fei EARS z, 分 别 为 
ZAR, 
š p.!$,-Z,X, 
根据 扩展 信息 滤波 En 原理 ， 上 述 两 式 的 更 新 方程 
Z, =P,x- +H; RH kZ ika tI, 
£, =Ê k-i +H ; R, : [v, +H, Xia Fê- +i 
式 中 : 冯 为 信息 状态 贡献 量 ， 石 A iTA. HELK 
i, =H; R, [v, +H $k] 
I,-H, R, H, 


AF: H, 为 观测 方程 的 偏 导 数 ， wi 为 新 息 Cinnovation), Hv, —y, — h(&;y ) ° 


由 此 可 得 预测 信息 状态 及 协 方差 为 
[m =P p 
Zik- = Pr- -[F,P, ,F; +C 
AP: F, 为 状态 方程 的 偏 导数 。 
当 采 用 UKF 算法 时 ， 首 先 定义 扩展 状态 及 协 方差 矩阵 
Ih ' L9 a 
由 此 得 到 2n_+1 个 样 点 
Xor“ 
z =+ Sonta ) 
asit (Vote ) 


(11-46) 
(11-47) 


(11-48) 
(11-49) 


(11-50) 
(11-51) 


(11-52) 


(11-53) 


(11-54) 
(11-55) 


AP: A-at (ns+k) 一 n,，n, 为 扩展 状态 的 维 数 ， 其 他 参数 的 定义 可 参阅 第 4 章 。 


样 点 的 权 值 分 配 为 


w™ =1/(22 + 2n,) 
A - AKA n,)*( - a?) T 
u(? 21/24 * 2n,) 
由 此 ， 利 用 UT 变换 可 得 信息 预测 方程 为 


2n, 
^ pee (m) ,x 
Sk bn 2 ul A ike 
i=0 


=A/(4+n,) 
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(11-56) 


_p-l 
Zirik = 有 + 


式 中 : X41 为 状态 的 预测 样 点 ， Pi 为 相应 的 协 方差 矩阵 ， 且 有 


iat FÉ) 


2n; ( ) T 
- e * - ^ x = A 
Pr = Zu; [25.9 Xk+ijk Tea Xk+ijk ] 
i-0 


在 EKF 算法 中 ， 观 测 的 预测 误差 自 协 方差 和 互 协 方差 分 别 为 
P, kn =E[Qyin 一 Freak Wk 一 Freak )] " N, P, Hi. 
Para Ela- ksk 9a 一 Joa )] & P.H fà 
通过 类 比 ， 利 用 式 (11-61) 可 将 式 〈11-53) 表示 为 
l;a H m Rea H+ =P P aH HE v H, mU Um Gu y” 
“Ppap Pgs Rin Eya) Pry)” 
AP: Pop 可 通过 式 (11-59). 计算 得 到 ;而 Pya 则 可 通过 UT 计算 为 


2na 
-- (c) ^ ^ T 
Paj = Š u EART — Xa JL cas 2» Yea] 
i=0 
式 中 


Mak = h [wakap J Je = -$u i k+l|k 
E, A (11-52) PHI i, 可 计算 为 
i, =H En Rn [u +H k+ Eksik ] 
=P; Pa Hy R Wen Hua Pory) Pir) r] 
=P; P, xyk+1 FAL E HP haul Qna) £a] 
为 了 能 利用 EIF 的 算法 框架 ， 可 定义 伪 观 测 矩 阵 
Hen 5 Pin Pyra 
由 此 可 得 信息 贡献 量 的 更 新 式 为 
i, 74, Ria [orn +H kny ] 
I4 =a Ri M... 
及 系统 状态 的 更 新 
XQ, 
在 多 传感器 观测 条 件 下 ， 第 s 个 (Cs=12…,$S) 传感器 的 信息 贡献 量 为 
i 47H, (S, y! [orn Y kny ] 
Liar) Rin) Hin 
由 此 可 得 多 传感器 的 融合 结果 为 
£4 =Z. +Y ik. 


s] 


(11-57) 


(11-58) 


(11-59) 


(11-60) 
(11-61) 


(11-62) 


(11-63) 


(11-64) 


(11-65) 


(11-66) 
(11-67) 


(11-68) 


(11-69) 
(11-70) 


(11-71) 
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Zi =Z ^ i. (11-72) 
$71 
AP: Za. Zua TEER (11-56) 和 式 〈11-57) 得 到 。 

式 〈11-69) 一 式 〈11-72) 构成 了 不 敏 信息 融合 (CUIF) 的 递 推 过 程 ， 对 于 其 他 滤波 
算法 如 CDKF、CKF， 可 采用 完全 类 似 的 融合 方法 。 另 外 ， 在 融合 策略 方面 ， 除 了 采用 
如 上 所 述 的 状态 融合 外 ， 也 可 采用 粒子 融合 帆 ' “的 方法 ， 由 于 这 方面 的 理论 相对 不 够 成 
熟 ， 在 此 不 再 细 述 。 


11.4 应 用 实例 


11.41 多 红外 传感器 融合 跟踪 


考虑 二 维 平面 内 的 只 测 角 跟 踪 问 题 ， 目 标的 运动 方程 满足 
X, = Fx, + w, (11-73) 
式 中 : 状态 x =[x, x, y, x] : 4ER F =diag{F, F}; 7 为 观测 时 间 间 隔 ，w, 为 零 均值 


4 3 
扰动 噪声 ， 其 协 方差 阵 O, = diag{O ,O } 。 另 外 ， 5 -| 中 a=, ddl 2). 


0 1 15;3 r 
采用 位 于 不 同位 置 的 $ 个 红外 传感器 进行 观测 ， 第 个 传感器 的 观测 方程 为 
ei -aran DEL (11-74) 
Xy — X, 


RP: Cy) 表示 大 时 刻 传感器 s(s=1,2,…,S) 的 位 置 ， vi 为 零 均值 高 斯 噪声 ， 即 
~ 人 (Vv;0,o?)， 且 假设 不 同 传感器 间 的 观测 相互 独立 。 

仿真 中 假定 目标 初始 状态 x, =[20km - 200m/s 12km 100m/s] , T=0.5s, q-2m/s?. 
采用 3 个 固定 位 置 的 传感器 进行 观测 (传感器 的 位 置 如 图 11.4 所 示 ), 观测 精度 o,=2mrad 。 
进一步 假定 初始 估计 误差 协 方差 矩阵 P,-diag([250? 50? 250? 50], L=2km , 采用 EKF 
和 UKF 集中 式 融 合 方法 进行 跟踪 ， 实 验 次 数 为 500， 所 得 结果 如 图 11.5 一 图 11.9 所 示 。 





8 10 12 14 16 18 20 
axis-x(km) 





图 11.4 传感器 配置 图 11.5 单 次 跟踪 示例 
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RMSE of position estimation for axis-x(m) 


0 20 40 60 80 100 120 
Time index 


图 1L6 xz 轴 位 置 估计 均 方 根 误差 





RMSE of position estimation for axis-y(m) 
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图 11.7 》 轴 位 置 估计 均 方 根 误差 





A € so 
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点 点 
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Uu 0 ue 5 - i 
Z 0 20 4 6 80 10 12 名 0 20 40 60 80 100 120 
Time index Time index 
图 11.8 x 轴 速 度 估计 均 方 根 误差 图 11.9 》 轴 速度 估计 均 方 根 误差 


由 图 11.5 一 图 11.9 中 的 结果 可 以 看 出 ， 采 用 3 个 传感器 观测 ， 进 而 利用 EKF 和 
UKF 进行 融合 跟踪 ， 可 以 得 到 良好 的 估计 效果 ， 说 明了 集中 式 融 合 方法 的 有 效 性 。 另 
外 ， 从 图 中 的 结果 看 ， 两 种 算法 的 性 能 非常 接近 ， 说 明 在 处 理 这 类 问题 时 具有 等 效 性 。 
对 于 只 测 角 跟踪 问题 ， 由 于 存在 可 观测 性 条 件 的 限制 ， 这 里 没有 给 出 单传 感 器 的 跟踪 
结果 。 


181 


为 了 进一步 考察 传感器 间距 对 跟踪 误差 的 影响 ， 表 11.1 比较 了 Z=0.5km,lkm， 
2km,4km 等 不 同 条 件 下 的 ARMSE (大 于 10s) 。 由 此 可 以 看 出 ， 随 着 传感器 间距 的 不 断 
增加 ， 跟 踪 精 度 也 不 断 提 高 ; 究 其 原因 ， 是 因为 当 工 较 大 时 ， 具 有 更 好 的 可 观测 条 件 ， 
从 而 降低 了 估计 误差 。 


表 11.1 不 同 传感器 配置 条 件 下 的 跟踪 结果 





11.4. ”多 雷达 传感器 融合 跟踪 


考虑 二 维 平 面 内 的 距离 扩展 目标 跟踪 问题 ， 假 定 目标 运动 满足 
x, = Fx, + W, (11-75) 


AP: ARE x, =[x, x, y, Y, #,] : ERE F -diag(FE, F1): 7 为 观测 间隔 ，w, 为 零 均 值 过 
4 3 
程 噪声 , 其 协 方差 阵 O, = diagfO ,0,(79 )2?} 。 PF, F, -| T | aà Ë /3 T 3, 





0 1 mu m 
采用 S$ 个 雷达 传感器 进行 观测 ， 第 s 个 传感器 的 观测 方程 为 
"y. yo HOS | | v 
z,7| pi |-h(x,)*v, 7| arctan(y;/x, ) | 十 | vçk (11-76) 
大 ¿Ecos é; Vi 
AP: x;=x, xS k BEARES Cx, ) 与 第 s 个 传感器 (x, ) 的 相对 运动 状态 ， 且 
X,X, + YY 
ñana rre (11-77) 


《为 目标 真实 的 径 向 长 度 ; vou. ve 和 wi 分 别 为 目标 距离 、 方 位 和 径 向 长 度 观测 
噪声 ， 且 都 满足 零 均值 高 斯 分 布 。 

仿真 中 假定 目标 初始 状态 x, =[16km 16m/s 8km 12m/s 60m]' ， 初 始 估计 误差 协 方 
差 矩 阵 书 =diag([1002 202 100? 20? 5?) , T-05s, ，g=lm/s* ，g,=0.02m 。 采 用 3 个 固 
定 的 传感器 进行 观测 ， 其 位 置 分 别 为 (- 2km,0) 、(0,2km) 和 (2km,0) ， 距 离 、 方 位 和 径 
向 长 度 的 观测 精度 分 别 为 10m 、0.3" 和 2m «. SK HEX. 11-32) 所 示 的 集中 式 融合 粒子 滤 
波 算法 进行 跟踪 ， 粒 子 数目 M=6000 ， 初 始 粒 子 集 通过 PB 产生， 实验 次 数 为 100， 所 
得 结果 如 图 11.10 一 图 11.15 所 示 。 作 为 对 比 ， 图 中 同时 给 出 了 位 于 原点 的 单传 感 器 跟 
踪 结 果 。 

由 图 中 的 结果 可 以 看 出 ， 通 过 粒子 滤波 算法 可 实现 对 距离 扩展 目标 的 有 效 估计 ， 且 
因为 粒子 具有 散布 性 特点 ,使 其 能 在 很 短 的 时 间 内 迅速 达到 稳定 的 跟踪 状态 。 相 比 之 下 ， 
多 传感器 融合 估计 的 精度 比 单传 感 器 要 高 ， 尤 其 是 对 于 目标 位 置 估 计 。 
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为 了 进一步 比较 粒子 数目 对 估计 性 能 的 影响 , 表 11.2 给 出 了 不 同 粒子 数目 条 件 下 跟 
踪 接近 稳定 后 (大 于 10s) 的 ARMSE。 可 此 可 以 看 出 ， 随 着 粒子 数目 的 增加 ， 估 计 性 能 
也 相应 地 有 了 不 同 程度 的 改善 ， 但 当 粒 子 数目 大 到 一 定 程 度 后 C M>6000 ) ， 增 加 粒 
子 数 目 对 跟踪 性 能 的 改善 不 再 明显 。 但 不 论 在 哪 种 条 件 下 ， 多 传感器 跟踪 精度 要 一 致 高 
于 单传 感 器 ， 说 明了 多 传感器 融合 跟踪 的 优势 。 
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图 11.10 单 次 跟踪 示例 
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图 11.11 长 度 估 计 均 方 根 误差 
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RMSE of position estimation for axis-x(m) 
RMSE of position estimation for axis-y(m) 
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图 11.12 x 轴 位 置 估计 均 方 根 误差 图 11.13 》 轴 位 置 估计 均 方 根 误差 
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1 sensor 
—.q sensors 





RMSE of velocity estimation for axis-x(m/s) 
RMSE of velocity estimation for axis-y(m/s) 
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图 11.14 x 轴 速 度 估计 均 方 根 误差 图 11.15 yy 轴 速度 估计 均 方 根 误差 
表 11.2 不 同 传感器 配置 下 的 跟踪 结果 比较 


1 sensor 3 sensors 


x posy velx vely len pos x posy velx vely len 


1102 2087 181 240 070 654 1259 148 157 0.51 


11.55 21.74 1.89 244 0.72 6.56 12.66 1.50 160 0.54 
13.10. 23.92 1.99 2.62 0.79 773 15.36 155  L65 0.68 
13.83 25.78 2.03 265 0.89 9.86 1991 1.65 167 0.90 





11.5 本 章 小 结 


本 章 主要 阐述 非 线性 贝 叶 斯 滤波 理论 和 方法 在 多 传感器 观测 条 件 下 的 扩展 ， 重 点 介 
绍 了 集中 式 和 分 布 式 这 两 种 融合 滤波 原理 ， 并 针对 集中 式 融合 结构 给 出 了 基于 解析 高 斯 
近似 和 随机 采样 近似 方法 的 融合 滤波 实现 过 程 和 应 用 实例 。 

仿真 结果 表明 ， 与 单传 感 器 相 比 ， 采 用 多 传感器 集中 式 融 合 的 方法 可 以 有 效 提高 滤 
波 精度 和 稳定 性 ， 特 别 是 提高 只 测 角 条 件 下 的 目标 跟踪 能 力 。 需 要 指出 的 是 ， 对 于 分 布 
式 融 合 ， 由 于 每 个 节点 传感器 的 滤波 算法 与 基于 传统 的 单传 感 器 滤波 算法 相同 ， 其 主要 
差异 主要 体现 在 中 心 节 点 对 各 传感器 估计 结果 〈 状 态 估计 和 协 方差 矩阵 ) 的 融合 策略 上 。 
由 于 这 部 分 内 容 不 是 本 书 分 析 的 重点 ， 本 章 没有 进行 深入 展开 ， 感 兴趣 的 读者 可 参阅 有 
关 文 献 ， 如 文献 [13] 和 文献 [14] 等 。 
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第 12 章 混合 系统 模型 滤波 


在 前 面 讨论 的 各 种 非 线 性 滤波 算法 中 ， 主 要 基于 单一 的 系统 模型 假设 ， 即 尽管 系统 状 
态 是 未 知 的 ， 但 其 演化 过 程 通常 只 有 一 种 特定 的 形式 。 但 在 真实 世界 中 ， 系 统 往往 会 因为 
有 不 同 的 输入 而 呈现 出 不 同 演化 过 程 ， 相 应 地 得 到 不 同 的 量 测 ， 如 在 目标 跟踪 领域 ， 出 于 
规避 或 某 种 特定 的 需要 等 目的 ， 往 往 会 执行 各 种 特定 的 机 动 方式 ， 在 战场 上 的 军事 目标 更 
是 如 此 。 在 这 种 条 件 下 ， 需 要 寻求 混合 〈Hybrid) 系统 滤波 估计 方法 50557 来 解决 问题 。 

对 混合 系统 进行 估计 通常 采用 多 模 (Multiple Model, MM) 方法 来 处 理 ， 具 体 实现 
时 需 针对 具体 的 混合 模型 形式 进行 。 对 于 模式 (Mode) 固定 的 系统 ， 即 系统 可 能 的 演化 
模型 个 数 固定 不 变 ， 且 不 存在 模式 迁移 (或 转换 ) 现象 。 在 这 种 情况 下 ， 往 往 采 用 静态 
ZER (Static Multiple Model) 估计 算法 即 可 ， 即 对 每 一 种 模式 ， 采 用 相应 的 模型 进行 独 
立 滤 波 估计 ， 最 后 对 各 个 基于 模式 的 匹配 滤波 结果 进行 综合 。 这 样 ， 正 确 的 模型 必然 是 
所 考虑 的 模型 当中 的 一 个 ， 且 为 模型 后 验 概率 最 大 的 那 一 个 。 

实际 中 碰 到 的 问题 往往 比 上 面 所 提 到 的 要 复杂 , 即 系统 模式 存在 转移 或 跳 变 (Jump) 
(如 目标 由 匀速 运动 变 为 匀 加 速 或 匀 转 弯 运 动 等 )， 在 这 种 条 件 下 ， 只 能 采用 动态 多 模 
(Dynamic Multiple Model) 方法 来 处 理 。 因 此 ， 下 文 如 非特 殊 说 明 ， 所 指 的 多 模 方法 都 
默认 为 动态 多 模型 方法 。 

多 模 方法 利用 多 种 状态 模型 来 描述 不 同 的 系统 状态 演化 模式 ， 在 系统 状态 矢量 中 增 
加 一 个 离散 变量 用 于 指示 该 模型 下 的 状态 演化 过 程 ， 实 现 系 统 状态 与 模型 属性 的 混合 估 
计 。 多 模 算 法 经 历 了 三 个 发 展 阶段 局 ， 每 一 个 阶段 在 结构 、 操 作 、 性 能 ， 以 及 应 用 潜力 
和 局 限 性 方面 都 有 着 较 大 区 别 。 第 一 代为 自 适 应 多 模 (AMM) 方法 ; 第 二 代为 协作 多 模 
型 (CMM) 方法 ， 最 具有 代表 性 的 就 是 交互 多 模 OMM) 方法 ; 第 三 代为 变 结构 多 模型 
方法 (VS-MM)。 

从 多 模 滤波 的 实现 方法 看 ， 主 要 有 基于 混合 高 斯 近似 的 方法 和 基于 随机 采样 近似 的 
方法 ， 将 书 中 第 二 部 分 介绍 的 混合 高 斯 近似 方法 与 IMM 相 结 合 是 实现 混合 系统 状态 估 
计 的 常用 处 理 手 段 ， 它 在 状态 估计 方面 有 着 良好 的 性 能 。 但 是 当 采 用 随机 采样 近似 方法 
与 MM 的 结合 则 存在 一 些 问 题 ， 第 一 ， 粒 子 滤波 本 身 是 一 种 密度 估计 技术 ， 而 不 是 直 
接 的 状态 估计 方法 ， 用 于 描述 后 验 密度 的 粒子 数量 大 ， 如 果 采 用 交互 方式 ， 算 法 复杂 性 
较 强 ， 第 二 ， 粒 子 滤波 算法 是 一 种 基于 蒙特 卡 罗 随 机 采样 的 近似 技术 ， 随 机 采样 对 于 描 
述 后 验 密度 ， 搜 索 并 逼近 真实 模式 有 着 重要 的 意义 ， 一 旦 采用 交互 处 理 ， 会 破坏 粒子 状 
态 转 移 的 随机 性 。 

鉴于 此 ， 本 章 主要 考虑 将 混合 高 斯 近似 方法 与 IMM 相 结 合 ( 称 为 传统 高 斯 混合 
交互 多 模 ，IMM) "””" ， 以 及 随机 采样 近似 方法 与 AMM 相 结合 〈 称 为 多 模 粒 子 滤 波 ， 
MMPF) 巴 鸭 的 混合 模型 滤波 算法 ， 并 针对 机 动 目标 跟踪 应 用 问题 给 出 了 具体 的 实例 。 
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121 混合 状态 的 贝 叶 斯 估计 


离散 时 间 混 合 系统 可 由 如 下 动态 方程 和 观测 方程 描述 
x, = f(x, 0 wa) (12-1) 
z, = hX, rV) (12-2) 
AP: 天 为 上 -1 到 大 时 刻 之 间 起 作用 的 模型 变量 ， 该 变量 通常 描述 为 时 间 均 匀 的 一 阶 马 
尔 可 夫 链 ， 其 转移 概率 为 
mi £P =j =} (7eMD (12-3) 
AP: Ms {1,2,…,m} , HERRER TI =[z,] 为 疾 x 严 方 阵 ， 且 和 矩阵 元 素 满足 


m; 过 0， Y x, =l (12-4) 
j=l 
对 于 每 一 个 5j eM ， 初 始 模型 概率 表示 为 
u, Š P(r, = i) (12-5) 
而 对 于 每 一 个 uu. WE 
Hu, 2:0, Syal (12-6) 
i=1 


式 〈12-1) 和 式 〈12-2〉 的 其 他 部 分 在 前 文中 已 有 过 描述 。 当 m=1 时 ， 式 (12-1) 
和 式 (12-2) 分 别 等 同 于 式 (1.1) 和 式 (1.2)。 而 由 式 (12-1) MA (12-2) 定义 的 非 线 
性 滤波 问题 ， 不 仅 要 估计 系统 状态 (连续 变量 )， 还 要 估计 模型 值 (离散 变量 )， 称 这 种 
类 型 的 问题 为 混合 状态 估计 问题 。 

在 贝 叶 斯 滤波 框架 内 ， 式 〈12-1) MA (12-2) 的 概念 性 解 也 与 式 (1.3) MR (1.4) 
类 似 , 其 区 别 在 于 现在 所 考虑 的 状态 矢量 为 混合 状态 矢量 y, =[xi ,nn]。 此 时 , 根据 贝 叶 
斯 定理 ， 其 递 推 处 理 过 程 可 按照 预测 和 更 新 两 个 步骤 描述 如 下 。 

预测 : 


p(x,.5 = jl 244) 7 2,7; [p(x, [x =J) P(X- Eil ara) (12-7) 
i 
更 新 : 


. p(z, | X,,7, = j)p(x,,r, - J|z44) 
m = |z 一 一 有 -ROGER a 
p(X =j |z) Y: [pa xon = J)p(xon = a (12-8) 
上 述 得 到 的 是 混合 系统 模型 估计 的 一 般 性 求解 表达 式 ， 具 体 的 求解 方法 依赖 于 具体 
的 模型 形式 。 鉴 于 混合 高 斯 滤波 和 粒子 滤波 在 处 理 非 线 性 / 非 高 斯 估计 问题 中 的 强大 能 
力 ， 接 下 来 首先 考虑 基于 混合 高 斯 近似 的 多 模型 滤波 算法 ， 在 此 基础 上 分 析 基 于 粒子 实 
现 的 多 模 滤波 算法 。 
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12.2 多 模 高 斯 滤波 算法 


HEARS On 个 ) 模型 集 条 件 下 的 混合 估计 问题 ， 根 据 全 概 公式 ， 系 统 状 态 的 后 
验 PDF 为 


p(x 1 a)=Y pe 1r, 94) Pr la) 


(12-9) 
=》， p, | aa us (j) 
j=1 
以 各 模型 为 条 件 的 状态 后 验 PDF 为 
p, |r 0) maa a) ZEO) px， In maa) (12-10) 


PG, In 0) maa) 
将 全 概 公式 再 次 用 于 式 (012-10) 中 的 第 二 项 ， 有 


p(x, 1n 0) 4a -Y px, |n.) a (ü), Prin, Oln 0)4.] (12-11) 
i=] 
在 高 斯 近似 假设 条 件 下 ， 式 (12-11) 可 进一步 表示 为 


px, |n O) £ p[x, 1n 9), (0), n Ka a ]u, [ly] 
Sq (12-12) 
=>, p[x, 1n G) (04, Pa Jaz- Uy] 
i=l 


X (12-12) 第 一 行 表示 前 k--1 个 时 刻 的 状态 可 近似 为 m 个 基于 模型 的 条 件 状 态 估 
计 之 和 。 

进一步 假定 该 混合 形式 为 高 斯 混合 分 布 ， 而 后 对 单个 高 斯 分 量 采 用 和 矩 匹配 近似 处 
理 ， 得 


p@ |ñ Goa) EN (xE 8a eov) un] 
i=l 


~ Na $ Erli Ak- lij cov o) (12-13) 


=N Ë E Ë In, 0.2. X uh [ij ] ,COV o) 
x (12-13) 最 后 一 行 利用 了 混合 高 斯 分 布 的 线性 性 质 ， 与 模型 j 匹配 的 滤波 器 的 输 
入 是 由 m 个 滤波 器 的 交互 得 到 的 ， 每 个 滤波 的 混合 分 量 为 名., ， 相 应 的 权重 或 概率 ) 
为 Li[ 滑 ] ， 称 为 混合 概率 。 
利用 式 〈12-13) 实现 的 多 模 滤 波 算法 称 为 交互 多 模 (OMM) 算法 ， 图 12.1 给 出 了 
该 算法 的 实现 结构 ， 包 含 m 个 并 行 运行 的 交互 滤波 器 ,混合 过 程 在 每 个 滤波 器 的 输入 阶 
段 完 成 ， 并 以 zk- 为 条 件 ， 混 合 概率 的 计算 将 在 下 文 的 具体 算法 流程 中 详细 说 明 。 
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图 12.1 IMM 估计 算法 


交互 多 模 滤 波 的 详细 算法 流程 可 概括 如 下 。 

Alg (12.1): Interacting Multiple Model Filtering 

步骤 1: 混合 概率 计算 

在 zx 和 时刻 模式 7 已 知 的 条 件 下 ，k 一 1 时 刻 模式 x 的 概率 为 
Hy aU] < Pr[r, , OG) ] 


-—Pr Oh Orta Prina Olaa] et 
| 
X (12-14) 的 混合 概率 可 进一步 表示 为 
Aa A" a) , ij=1,2,---,m (12-15) 
式 中 ， 归 一 化 常数 5OF-》 lm na O. 
i=l 
步骤 2: 模型 混合 
由 总 出 发 ， 根 据 式 (12-13) HES r (O 相对 应 的 混合 初始 条 件 ， 即 
ia OY i ADN] s J712,-m (12-16) 
i=l 


相应 的 协 方差 为 
P, (0/)-Y' Hk- [ij] (8. E s", X, ; = x y +a ), j=1,2,…,m (12-17) 
ivl 


步骤 3: 模型 匹配 滤波 
利用 步骤 2 的 结果 作为 模式 0) 的 匹配 滤波 的 输入 ， 在 量 测 z, 的 作用 下 得 到 总 和 


P/ ， 对 应 于 m 个 滤波 器 的 似 然 函数 为 
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A, ()-p[z,|r, G), 4] p[z lr G) Xr (0)),P, ,(07)] 
= 人 [zx Eua (j),S, 0)] 


步骤 4: 模式 概率 更 新 


(12-18) 


H, () È Prir, (Iz - plz br Oz Pris Oaa] 
-EA OS pb. On) Prin i Oaa] (12-19) 
i=1 
-TA DD JO 


RH: PAUZO). 
DRS: 状态 和 协 方差 估计 


基于 模型 的 条 件 估计 及 协 方差 为 
x, -Y X0), 0) (12-20) 
j=l 
P =F m UNG - £8] - $, +P), J=12,--..,m (12-21) 
ja 


需要 指出 的 是 ， 步 又 5 只 用 于 结果 输出 ， 并 不 参与 递 推 计算 。 另 外 ， 式 〈12-18) 中 
包含 了 量 测 的 预测 2,4 0). 及 预测 误差 协 方 差 矩 阵 S.G) ， 这 两 式 的 计算 依赖 于 具体 的 观 


测 模型 和 非 线性 滤波 算法 ， 典 型 的 有 EKF, UKF 或 QKF 等 ， 相 应 地 得 到 基于 
EKF/UKF/QKF 的 IMM 估计 算法 。 


123 多 模 粒 子 滤波 算法 


对 式 (12-1) MA (12-2) 求解 也 可 采用 基于 SMC 近似 的 多 模 粒 子 滤波 来 实现 。 定 
义 新 的 状态 向 量 为 y, = [xz 并， 其 中 eM = {1,2,…,m} 。 且 假设 已 知 初始 密度 p) 和 
p(n)- 2 uó(n- i)» 335b. E WP wP yrs ARRERA p(y | zx) 的 粒子 集 。 则 


多 模 粒 子 滤波 算法 可 概括 如 下 。 
Alg (12.2): Multiple Model Particle Filtering 


[D uy, ] - MMPF[ of uf. | 

e。 按 照 Alg (12.3) 进行 模型 状态 转移 
[ey ]- Mr[ ys | 

oik Alg (12.4) 进行 模型 的 SIS 滤波 

| wP ya |= M-sis| (xr uo | 
。 计 算 有 效 样本 数 育 7 

eif Ñy <N, 

[6 NY |= resample| p, wf? yz. | 


eend if 
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MMPF 算法 的 第 一 步 是 使 用 时 刻 上 -1 的 模型 粒子 集 o0), 和 转移 概率 矩阵 
II = (z) 产生 时 刻 上 的 模型 粒子 集 {%j- ， 其 实现 过 程 如 Alg (12.3) 描述 的 模型 变量 
状态 转移 (Model Transition, MT) 算法 。 
Alg (12.3): Model Transition 
[e ]|2 Mr[ oan | 
efori-l:m 
— c,(0) 20 
— forj=1:m 
* c) » c -D + zj, 
— end for 
e end for 
eforn=1: N 
一 产生 均匀 分 布 数 u, ~ U[0,1] 
-4 L= ii 
— m, =1 
— while (c;(m;) « wu, ) 
* mo =mo +l1 
— end while 
wa 4 p? =m, 
eend for 


MT 算法 的 基本 处 理 步骤 是 : Rr =i，u, ~ U[0,1] ， 当 下 式 成 立时 ，n” 的 取 值 


Am,eM. 
-1 
Fa <u, «Ys, (12-22) 
j=l 


ja 


mo 
RP: 2 z; 表示 离散 随机 变量 六 在 给 定 关 ,=i 条 件 下 的 累积 分 布 函数 。 
j=l 


多 模 粒 子 滤波 算法 的 第 二 步 是 进行 基于 模型 的 序 贯 重要 采样 滤波 (Model Based SIS 
filter，M-SIS)， 具 体 实 现 过 程 可 总 结 如 下 。 
Alg (12.4): Model Based SIS Filtering 
[ Gf? wf? yz, ] =M-SIS| 6n of Yos | 
eforn -1: N 
-粒子 采用 ， ”~gG@% | xp rez) 
一 计算 重要 密度 权重 
w =y” pG, EMUN pa” Laki) 
k k-l (m) (m 
q(x, | xi, 5 Zk) 


eend for 
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N 
。 计 算 权重 和 ww = wP 
n-l 


eforn=1: N 
-权重 归 一 化 : uj -ww,, 
eend for 
Alg (12.4) 中 的 最 优 重 要 度 密度 函数 为 
q xir 1 Zu us = pos | XE» 8) (12-23) 
而 通常 将 其 选 为 先 验 状态 转移 密度 函数 
qG, 1x57). n0). 2,) = p, | xs ni) (12-24) 
在 MMPF 算法 中 ， 一 个 重要 的 现实 问题 是 ， 如 果 各 个 粒子 按照 各 自选 择 的 模型 进行 
状态 预测 ， 粒 子 滤波 是 否 能 够 保证 上 时 刻 的 粒子 集 能 够 准确 描述 该 时 刻 的 目标 状态 分 布 。 
众所周知 ， 粒 子 权 重大 小 表示 了 粒子 状态 接近 真实 状态 的 程度 。 若 用 MMPF 算法 解决 混 
合 系统 的 状态 估计 问题 ， 在 粒子 状态 中 增加 了 表示 状态 模型 的 变量 ， 滤 波 过 程 中 ， 先 通过 
大 -1 时刻 的 粒子 模型 变量 值 和 模型 转移 概率 来 预测 上 时 刻 的 粒子 模型 变量 值 . 然后 ,，k 一 1 
时 刻 的 各 个 粒子 依据 新 模型 变量 值 对 应 的 状态 转移 方程 进行 粒子 状态 预测 ， 得 到 上 时刻 的 
粒子 ， 并 计算 粒子 权重 。 此 时 ， 各 个 粒子 权重 大 小 不 一 ， 符 合 目标 真实 运动 模型 的 粒子 状 
态 将 接近 于 真实 状态 ， 其 权 值 相对 较 大 ， 反 之 ， 则 远离 目标 真实 状态 ， 权 重 较 小 。 在 这 种 
情况 下 ， 可 以 发 挥 重 采 样 方法 所 固有 的 作用 ， 重 复权 重大 的 粒子 ， 剔 除 掉 权重 小 的 粒子 ， 
这 样 , 将 会 有 大 多 数 粒 子 的 模型 变量 值 等 于 真实 值 , 粒子 的 分 布 更 接近 目标 真实 状态 分 布 。 
下 面 通过 一 个 简单 的 示例 ， 说 明 多 模 条 件 下 的 粒子 预测 及 重 采 样 过 程 对 模型 的 选择 能 力 。 
如 图 12.2 所 示 ， 假 定 上 -1 时 刻 目 标的 真实 状态 为 x_i=[100 60100 60] , H 100 个 
散布 在 真 值 位 置 附近 的 粒子 近似 目标 的 状态 分 布 ， 散 布 方差 为 已 ,=diag([5 1 5^ °], 
分 别 用 匀速 直线 模型 、 顺 时 针 匀 转弯 模型 及 道 时 针 匀 转弯 模型 进行 预测 (采样 间隔 为 3s， 
转弯 率 为 4"/s )， 得 到 的 粒子 云 分 布 情况 如 图 (a) 所 示 。 进 一 步 假 设 在 上 -1 到 大 时 刻 目 
标 按照 模型 1 运动 , 大 时 刻 目标 的 真实 状态 为 x_1=[280 60 280 60] ， 而 此 时 得 到 的 目标 
位 置 观测 为 (281.54,277.75) ， 观 测 误差 协 方差 为 R=diag([25 25]) 。 图 12.2 (b) 为 根据 大 
时 刻 的 观测 进行 权重 计算 ， 重 采样 之 后 的 粒子 分 布 。 


` Xk. 
Xk- (model 1) 
xkk (model 2) 
* XQ (model 3) 





50 100 150 200 250 300 350 
axis-x 
(a) 重 采 样 前 
图 12.2 多 模 粒子 的 预测 与 更 新 过 程 
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3 Xka 
Xy (model 1) 
Xy (model 2) 
Xy. (model 3) 





50 100 150 200 250 300 350 
0) 重 采样 后 
图 12.2 多 模 粒子 的 预测 与 更 新 过 程 ( 续 ) 
从 图 12.2 中 的 结果 可 以 清楚 地 看 出 ， 多 模 可 以 充分 挖掘 目标 的 运动 状态 变化 ， 而 粒 
子 滤波 算法 通过 重 采 样 步 又 能 够 有 效 选择 模型 变量 ， 使 其 逼近 真实 的 目标 运动 状态 。 


12.4 应 用 实例 


12.4.1 交互 多 模 机 动 目标 跟踪 
假设 二 维 平面 中 的 目标 运动 方程 满足 
x, = Fx, , * Tu, + Gw, , (12-25) 
RP: x,=[x, y, X, yk] 29 k HARE Eb AS mla a,] 29188394. FRERE: 
[为 控制 输入 矩阵 ，C 为 噪声 输入 矩阵 ， w,. 为 零 均 值 高 斯 过 程 噪声 ; H 


1000 T2 9 
[0100 „alra g w 
""p n Fu tou pt rh 
0007 0 T 
采用 主动 雷达 传感器 ， 观 测量 包括 目标 距离 六、 距离 变化 率 六 及 方位 网， 观测 方程 
可 表示 为 
xc " 
he t= Sene M, (1226) 
Xy ty, Vox 








arctan(y,/x, ) 
AP: n 为 零 均 值 高 斯 噪声 ， 且 cov[w]=R, 。 
假设 目标 的 初始 状态 为 x =[80km 20km 一 200m/s 0m/s]' ， 过 程 噪声 协 方差 矩阵 
Q, -diag([(1m/s?) (Im/s2)2) ， 采 样 间隔 7=1s 。 目 标的 航 迹 分 为 5 段 ， 每 一 段 的 运动 状 
态 分 别 描述 为 : 
第 1 段 (0s 一 40s): 以 x 为 初始 状态 的 匀速 运动 。 


192 


582 BR (41s—90s): 第 一 次 机 动 ， 加 速度 w=[0.6g 0.8g] (8 为 重力 加 速度 )。 

第 3 段 (91s 一 140s): 以 第 一 次 机 动 后 为 初始 状态 的 匀速 运动 。 

第 4 段 (141s 一 180s): 第 二 次 机 动 ， 加 速度 z=[- 0.6g -0.8g] 。 

# 5 BE (181s 一 240s): 以 第 二 次 机 动 后 为 初始 状态 的 匀速 运动 。 

采用 包含 CV 和 CV 两 种 模型 的 交互 多 模 (IMMO 算法 进行 跟踪 ， 具 体 应 用 时 ， 由 
T CV 模型 的 状态 量 维 数 为 4, 为 此 需要 对 状态 量 进行 扩展 , 得 到 用 于 跟踪 的 CV 模型 为 


107000 T!|2 0 
010700 0 Pn 
01 000 T 0 
muU UV 0 0 1 o o * 0 T Wi 
000000 0 0 
000000 0 0 


式 中 
x, = [x y x X y] (&) 
同 理 ， 得 到 用 于 跟踪 的 CA 模型 为 


10 T 0 T 0 T: © 
010 T 0 Pn o T2 
0010 T T 0 

% m Fons], 00 1 0 Xk- t 0 了 W, 
0000 1 0 1 0 
0000 0 1 0 1 


假定 初始 状态 估计 误差 协 方差 矩阵 为 Pl!=P?=diag([300” 300? 100? 100? 2? 2*])， 初 


0.95 0.05 
始 模型 概率 [Am(D (2)]=[0.95 0.05] , BEEREK T= oro si 距离 、 方 位 及 


径 向 速度 观测 精度 分 别 为 o,=5m 、o。=0.2 和 =0.2m/s 。 采 用 随机 初始 化 方法 产生 初 
始 状 态 估计 ， 利 用 第 4 章 中 介绍 的 UKF 算法 进行 跟踪 滤波 ， 实 验 次 数 为 500， 所 得 结果 
如 图 12.3 一 图 12.6 所 示 。 

30 


— — True 
20 *- Estimated 
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图 123 单 次 跟踪 示例 图 12.4 模型 概率 估计 
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RMSE of position estimation(m) 





100 — 150 200 


Time(s) Time(s) 
图 12.5 位置 估计 均 方 根 误差 图 12.6 速度 估计 均 方 根 误差 


由 图 12.3 一 图 12.6 中 的 结果 可 以 看 出 ， 采 用 IMM 算法 可 以 对 机 动 目标 进行 有 效 跟 
踪 ， 不 过 从 图 中 可 以 看 出 ， 在 机 动 发 生 时 刻 跟踪 误差 有 短 时 上 升 的 现象 。 这 是 因为 跟踪 
过 程 隐 含 了 对 未 知 加 速度 的 估计 ， 这 个 估计 从 初始 建立 到 稳定 收敛 有 个 过 程 ， 从 而 使 估 
计 误 差 出 现 先 增 后 降 的 趋势 。 另 外 ， 从 图 12.4 中 的 结果 可 以 看 到 ， 在 目标 处 于 近 匀 速 阶 
段 ， 匀 加 速 运动 模型 的 模型 概率 并 不 接近 零 ， 这 是 因为 匀 加 速 运动 模型 包含 了 目标 的 匀 
速 运动 ， 因 而 具备 近似 匀速 运动 的 能 力 ， 这 种 现象 即 所 谓 的 存在 模型 竞争 现象 。 只 不 过 
在 这 种 条 件 下 ，CA 模型 是 失 配 的 ， 因 而 模型 概率 不 如 CV 模型 高 。 


12.4.2 多 模 跟 踪 的 粒子 实现 
考虑 二 维 平面 内 的 目标 检测 前 跟踪 问题 , 假设 在 观测 时 间 内 目标 的 运动 满足 以 下 种 模 
型 中 的 一 种 :QD 匀速 运动 ，@ 顺 时 针 转 弯 ，@ 逆 时针 转 弯 ， 其 运动 状态 可 用 通 式 表示 为 
Xa = FP)x, +w, (12-27) 
AP: x, S o, X, y, Y. L1 为 状态 矢量 ; wi 为 过 程 噪声 且 w ~ N(0,Q,); r, e M=(1,2,3) 
为 上 -1 到 k 时 刻 之 间 的 模型 状态 变量 ; eO 为 相应 的 状态 转移 矩阵 , 针对 以 上 3 种 模型 ， 
其 转换 矩阵 的 具体 形式 为 





17T 000 
01000 
F?Ó?-|o00 1 T 0 
00010 
0000 1 
1  sin(oPTyoP — -(1-cos(o? T) o? 0 0 
0 cos(aX?T) 0 sin(o?T) 0 
F9?-|9 (1-cos(o(?T)y/of? 1 sin(oP?TyoP? 0|, s=23 
0 sin (@®T) 0 cos(@” T) 0 
0 0 0 0 1 
式 中 :7 为 传感器 扫描 周期 wp-= -天 二， oP ==, a, 为 机 动 加 速度 。 
X, Ty, x, ty, 
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在 某 一 特定 时 刻 ， 目 标 只 遵循 一 种 运动 模型 ， 但 在 整个 运动 过 程 中 ， 目 标 运动 可 在 各 
种 模型 之 间 相 互 转换 ， 其 转换 规律 可 用 马尔 可 夫 转 移 概率 矩阵 来 描述 ， 该 矩阵 可 定义 为 
H1 [2 Hs 
IT,-| haa ps 
Ha AL 3 
式 中 : Ly(iy=1,2,3) 为 模型 i 到 模型 j 的 转移 概率 。 
kk 时刻 目标 可 能 存在 于 传感器 警戒 区 域内 ， 也 可 能 不 存在 ， 可 用 变量 E, e {0,1} 进行 
ER. HF 0 表示 目标 不 存在 ，1 表示 目标 存在 。 此 外 ， 新 生 目 标 和 消亡 目标 的 概率 分 
别 为 
P, 5 P(E,=l|E,_,=0) 
F, š P (E,=0JE,_,=1) 
相应 地 ， 转 移 概 率 矩 阵 为 
-3 R 
a 4 P, ud 


设 在 一 个 大 小 为 xn, 的 警戒 区 域 和 内， 传感器 的 观测 方程 为 
Gj) GJ) — pn». 
z^) e re (x,)+ v, s) E (12-28) 
_ GA, -xy * (A, - y) 
2x' 

di WW 为 第 (i 四 个 分 辨 单元 的 观测 噪声 。 

下 面 结 合 上 述 系统 模型 具体 分 析 多 模 粒 子 算法 的 实现 过 程 ， 为 此 首先 引入 状态 变量 
y, 2 bx. n] 。 假 设 已 知 在 上 -1 时 刻 的 后 验 概 率 密度 py, ila a) THATE (Ow Y, 
来 表征 N 为 粒子 数目 )， 则 其 完整 的 一 步 递 推 过 程 可 概括 如 下 。 

Alg (12.5): MMPF Based Track-before-Detect 

步骤 1: 存在 变量 预测 

根据 不 ”和 转移 概率 矩阵 JT. ， 得 到 kk 时刻 存在 变量 E07(n=1,2,…,N) 。 

步骤 2: 粒子 状态 预测 

这 里 仅 对 EO = 的 粒子 进行 预测 ， 分 两 种 情况 分 别 进行 处 理 。 

OD 对 “新 生 ” 粒 子 〈 即 满足 EP -0 EP-IELTO, FLERE GP yp) 
择 强度 超过 预 设 门限 7 的 网 格 单元 集 Cz y )， 使 粒子 在 这 些 单元 内 随机 分 布 ， 对 于 
速度 变量 Qu De). ， 则 使 其 在 [- Vna vnax ] 之 间 均 勾 分布， 同样 强度 分 布 满足 I”~ 
Ull nin Ins]: 同时 根据 初始 模型 概率 假设 条 件 ， 随机 从 模型 集 M 中 按 一 定 的 模型 概率 产 
生 该 粒子 的 运动 模型 变量 。 

(2) 对 于 “存活 ”粒子 〈 即 E 中 =1，E 中 =1 )， 首 先 通 过 前 一 时 刻 该 粒子 的 模型 变量 
r K IT, 获取 当前 时 刻 的 模型 变量 mw? ， 然 后 再 根据 mA 所 对 应 的 目标 运动 方程 进行 粒 
子 状态 预测 。 


ETT, 
AP: hU imo, y CUP ， 了 为 信号 强度 ，Z 为 扩散 因 
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步骤 3: 粒子 权 值 计 算 
采用 似 然 比 形式 计算 粒子 的 权重 ， 归 一 化 前 的 权重 为 
[I (^x — po. 
k 


e ees FEm-0 
RP: GEP- pP oiibtP ， 训 表示 最 接近 xW"[1] 的 整数 ，P 为 受 目 标 影响 的 单元 
^3, C, GP?) 的 含义 与 此 类 似 。 完 成 粒子 权 值 计 算 后 得 到 归 一 化 的 权 值 为 ut - 


N 
ur 5 wt? ñ 


步骤 4: 粒子 重 采样 

重 采样 可 选择 性 进行 ( 即 当 有 效 粒子 数 小 于 某 一 特定 值 时 才 考 虑 重 采样 ), 也 可 强制 
进行 ， 经 重 采样 后 得 OP, N 1 7,7 Resample( {yP wy). 

步骤 5， 估 计 输出 

首先 估计 目标 的 存在 概率 ， 即 


P MN Ep 
e N e k 
EEA FHIR F, 则 认为 目标 存在 ， 进 而 估计 目标 的 状态 
&-Y PEPYS gp 
n-l 


n=l 


同时 , 对 于 EP 的 粒子 , 分 别 统计 模型 指示 变量 加 =1 0? 72 及 7 名 =3 的 数量 N. ` 
N 和 Na ， 由 此 得 到 大 时 刻 3 种 不 同 运动 模 型 的 概率 估计 


N N N 
P a-N/ ED, BaN’ ED, B L =N, EP 
n=] n=l n=l 


下 面 通 过 对 机 动 目标 的 检测 前 跟踪 仿真 验证 算法 的 有 效 性 ， 实 验 中 假设 扫描 周期 
7=ls ， 过 程 噪声 协 方差 矩阵 为 
37 $T 0 0 
2T aT 0 0 
Q- 0 0 37 2T 
0 0 27 qT 0 
0 0 0 0 qT 
XP: 470001; 92=0.01 。 仿 真 持 续 时 间 为 4$s， 目 标 实际 存在 时 间 为 30s (其 中 6s— 
15s 为 匀速 直线 运动 , 16s 一 20s 为 逆 时 针 转 弯 机 动 , 21s 一 30s 恢复 为 匀速 直线 运动 , 31s— 
35s 为 顺 时 针 转 弯 机 动 ， 目 标 在 36s 以 后 消失 )， 目 标 出 现时 的 初始 位 置 、 速 度 及 信和 号 强 
度 分 别 为 (20,20) ，(2,0) 和 五 =20 ， 其 他 参数 设置 见 表 12.1. 


表 12.. MMPF-TbD 滤波 算法 参数 设置 


© c o 

















ERE | mm | At, ann [oz [RT |P m | im | vm | an 
samj s | ı | oo [ie | oz] oé [o |?» | is | as | 2 [o 
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进一步 假定 目标 初始 时 刻 的 存在 概率 为 0.05,“ 新 生 ” 粒子 的 3 种 运动 模型 概率 分 别 
为 0.9、0.05 和 0.05,“ 存 活 ” 粒 子 的 模型 转换 概率 矩阵 为 
0.90 0.05 0.05 
010 0.80 010 
010 010 0380 


仿真 中 采用 N=60000 个 粒子 进行 跟踪 ， 所 得 结果 如 图 12.7 一 图 12.9 所 示 。 

由 图 12.7 中 的 结果 可 以 看 出 ， 在 仿真 参数 设置 条 件 下 (对 应 12dB 信 品 比 ), 通过 传 
感 器 观测 得 到 的 图 像 很 难 直接 判断 是 否 存 在 目标 。 但 从 图 12.8 中 可 以 看 到 ， 通 过 多 模 粒 
子 检测 前 跟踪 算法 ， 可 成 功 检测 到 目标 的 出 现 和 消失 ， 且 在 本 实例 的 参数 设置 条 件 下 ， 
对 目标 出 现 的 检测 无 时 延 ， 而 对 目标 消失 的 检测 仅 存在 一 个 时 刻 的 时 延 。 图 12.9 的 结果 
则 表明 ， 在 正确 检测 的 前 提 下 ， 多 模 滤波 算法 可 实现 对 目标 的 正确 跟踪 ， 且 在 机 动 发 生 
阶段 未 出 现 明 显 的 误差 增 大 现象 , 说 明了 滤波 算法 的 有 效 性 。 对 多 次 实验 进行 统计 分 析 ， 
可 得 出 类 似 的 结论 ， 在 此 不 再 装 述 。 


IT,= 











k " z k 
10 20 30 
axis-x axis-x axis-x 
(a) 第 5 帧 (b) 第 15 帧 (c) 第 25 帧 


图 12.7 不 同时 刻 的 观测 图 像 
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i | i + True 
40 J. : : o Estimated |. 


axis-y 





Estimatedprobabilityofexistence 


15 i : i I : 
0 5 10 15 20 25 30 35 40 45 15 20 25 30 35 40 45 
Time index axis-x 
4128 目标 存在 概率 估计 图 12.9 目标 跟踪 结果 


12.5 本 章 小 结 


本 章 主要 讨论 非 线性 滤波 理论 在 多 模 条 件 下 的 扩展 ， 首 先 给 出 了 贝 叶 斯 滤波 框架 下 
多 模 滤波 问题 解 的 一 般 表 达 式 ， 在 此 基础 上 重点 阐述 基于 混合 高 斯 近似 的 多 模 滤波 计算 
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问题 ， 并 以 交互 多 模 OMM) 为 代表 对 其 具体 的 实现 方法 进行 了 详细 介绍 。 作 为 多 模 滤 
波 的 另 一 种 重要 的 实现 手段 ， 本 章 还 对 基于 粒子 实现 的 多 模 滤波 算法 进行 了 详细 分 析 ， 
并 通过 实例 验证 粒子 重 采 样 过 程 在 模型 选择 中 的 作用 。 

仿真 结果 表明 ， 采 用 高 斯 混合 近似 和 随机 采样 近似 与 多 模型 方法 相 结 合 ， 均 可 实现 
对 机 动 目标 的 跟踪 。 相 比较 而 言 ， 高 斯 混合 多 模 滤 波 方法 运算 复杂 度 较 小 ， 但 应 用 范围 
不 如 粒子 滤波 方法 广泛 (如 检测 前 跟踪 应 用 通常 只 能 通过 粒子 滤波 的 方法 来 实现 )。 因此， 
实际 应 用 中 需 根据 具体 的 问题 选择 合适 的 算法 。 一 般 而 言 ， 适 合用 高 斯 混合 近似 处 理 的 
问题 ， 应 尽量 避免 用 复杂 度 较 大 的 多 模 粒 子 滤 波 方法 来 处 理 。 另 外 ， 本 章 介 绍 的 几 种 滤 
波 算法 只 是 多 模 滤波 方法 中 最 典型 、 最 重要 的 方法 ， 关 于 这 类 算法 的 更 多 介绍 和 研究 ， 
可 参阅 文献 [23] 一 文献 [26]。 
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附 3x 


附录 A ABER 
AJ 分 块 矩 阵 求 逆 公 式 


A B 
Ensem- "4 "An, WA 
a [4 +A DE CA" -A DE™ | 
M = eere " , E-D-CA'B CA-1) 
A. 和 矩阵 求 逆 引 理 
FERA, CHX, WA 
(A- BCD)! = A` - A! B(DA`'B + C™)DA™ (A-2) 


附录 B 3X (1-78) 证 明 


H S,=S, 4U — Sia (5) (Q5 HR.) HS, I] 可知 
SeSi -S,4 I — Sha H (LE) (L,+R,) HS, 
U — Sip- H'[(L,+R,)'] L; HS, Sie- 
Spp U — Si, HT (05) (L, +R) HS, — SH. (E, *R,) E, HS, 
$85, HI (4) (LR) HS, S H'[(L,+Ri) T. Le HS Sp id 
=P- Pa HT (05) (LRL) +I +R) T'E 
- (L) (+R) HP, ,H'[(L *R;) | E; MH Py, 
又 根据 HP,, ,H* -L,L, -R,, f8 
(LE) +R.) + HR T E; - 05) +R) HP, H LR) TL 
-(L,) (+R) {Li (L+R) HL, +R L; — LLL; +R} (L+R `L. 
=() (L,*R,) 4L; 4L, (RE) +R, Li *R, (L+R) TL. (B-2) 
7) G, HR.) (L+R AL +R. L+R) T'E 
SEOL SLL 
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将 式 〈B-2) RAJ (-D, £8 
S,S; Pa: PH (L,L, )'H "Baa 
一 和 -1 一 P, ,H* (HP, ,H* *R, )'H'Pa, (B-3) 
=P. - K, HP, ,—P, 


附录 C X (4-42) 导出 


对 于 滤波 估计 问题 ， 测 量 更 新 的 目的 是 在 给 定 状态 预测 和 观测 量 等 信息 的 基础 上 寻 
求 更 优 的 状态 估计 。 若 将 艺 和 了 视 为 相互 独立 的 多 维 高 斯 随机 矢量 的 实现 ， 则 有 
X ~N(x,P,), y— N (h(x), R) (C-1) 
AP: xx, 分 别 表 示 当 前 状态 及 其 估 值 ， 了 表示 实际 观测 值 ，ACx) 为 测量 模型 。 
为 表示 方便 ， 将 两 矢量 合成 为 一 增 广 的 矢量 形式 ， 于 是 有 


ZHI T (C-2) 
$, x 
HK (C-1) 及 前 面 的 独立 性 假设 条 件 ， 得 
R O 
z~ N(g(x),S), s-[ 可 (C-3) 


于 是 ， 更 新 问题 转化 为 在 给 定 z 、g(x) 和 SS 条 件 下 ， 寻 求 最 优 状 态 估 计量 x 的 问题 。 
若 用 自由 变量 6 代替 xX， 不 难 建立 下 列 似 然 函 数 


A(¿) = const - exp É -g(£)' S" (z- «| (C-4) 
X 的 最 大 似 然 估 计 GgROLTSVEO 可 表示 为 
x° - arg max[A(£)] (C-5) 
A (C-5) 的 等 价 表达 式 为 
x° -argmin[q(£)] (C-6) 
式 中 | 
44) - (£0) S -E CC-7) 
显然 ， 要 得 到 确切 的 上 使 之 满足 式 〈C-6) 是 不 容易 的 ， 但 若 有 
a(n) <q(Z;) (C-8) 


WETAH aE) E q(5,) B Bore Abd, EROR, Sa 就 比 5j 更 接近 最 优 解 。 
由 式 〈C-7) 和 式 〈C-8) 可 得 到 如 下 不 等 式 
[y -ME a Ry- (£,0]* ($, -£,)' P, (€, Ea) 
«[y -h R"[y - &(£,)] * (6$; -E P; (3, — £) 
若 序 列 (S; BARE, ETER (4-42). 


(C-9) 
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附录 D 式 (4-43 ) 导出 


对 于 正定 矩阵 序列 (P) ， 其 中 P=[p 收 ]eR” ， 若 对 于 j=1,2,… 都 有 不 等 式 
Pj, < PP 成 立 ， 则 可 以 证 明和 矩阵 序列 {P,} 的 极限 存在 。 证 明 过 程 可 分 为 两 步 ，@ 和 矩阵 序列 
对 角 元 素 p (m = 1,2», L) 的 极限 存在 ; @) 任 意 元 素 PU (m,n m= 1,2,---, L) 的 极限 存在 。 
ESI): 
4 e, =[0,…,0,1,0,…,0] "为 工 维 单位 矢量 ， 其 中 第 m 维 的 元 素 为 1 其 他 位 置 为 0。 
由 已 知 条 件 忆 ,< 已 ， 易 得 eIP,e, <eIPe,， 即 
We < iA 
BERTAS, (PP 为 单调 递减 序列 ， 同 时 该 序列 是 有 界 的 ， 即 0 入 pO < pO, 
故 该 序列 的 极限 存在 ， 即 有 lim Pam = Cmm( cn 为 一 常量 )。 
证 明 @): 
对 任意 两 单位 矢量 e Me, PaP, WE 
(e; * e; )P,. (e, +e,)< (e, +e, )P. (e, +e,) 
注意 到 P, M P ARRE, FER 
PES + PED 2pt < pH + pO -2pf 
利用 @ 中 的 结论 ， 显 然 有 1lim pn = Cnn. 
ZADAO, WAEREA UP) KREE, H lim P, = lim[pwn]=[cwn]=C。 


IMKE 3X (8-5) 导出 


对 于 式 (E-1) 和 式 (E-2) 所 示 的 系统 方程 


X, = f (x) + W, w, ~ N (0,Q,) (E-1) 
y, = H,x, +v, v, ~ N (0,R,) (E-2) 

AFP: 状态 量 和 观测 量 的 维 数 分 别 为 m 和 nn。 根据 贝 叶 斯 准则 ， 有 
p(x, | xi, Vk) = POX | x, )p(x, | x, )/p(y, | xia) (E-3) 


ARRA, KARS = f, x¿M, x, = |x, ， 于 是 式 〈E-3) 可 写 为 
[det(H,Q, Hz + R.)]2 expt-0.St]x, - fela +|y, - Axha D 


(E-4) 
[(27)” det(Q@,)det(R,)]2 exp(-0.5|x, — H, I PN jd 


py, |x, i, yk) = 


注意 到 
|x, -fl +y: - Bax l: =|x, -P,N, M + k. +y kk. -[N. la (E-5) 
AP: P'=0;' +H[R/ H,; N,=0; f, +H; R'y, o MitS D, 2 H,QH, +R,, 并 
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XX (E-5) 右边 的 后 三 项 进行 复杂 的 简化 运算 ， 最 终 可 得 
Maloe + -INils, = Di y + f Hi Di H, fe 
- fA HID y, — y, D, H, f, (E-6) 
- (y, - H,f,) D, (y, -H,f,) 
进一步 ， 式 (E-4) 变 为 
[det(H,Q,H + R,)] ? exp(-0.5|x, - P-N, |.) 
[(2m)” det(Q, ) det(R, )] ° 
i K, -P,H,R, ， 由 块 矩 阵 计算 式 ， 得 


= i = T " 
ET SE as > Ë: d E Tl A (G5 


-R, H, E 
I offo? L 0 
m @ P ” =Á  (E9 


p, | Xk) = (E-7) 


QF +H; RH, -HeRe |. 
-R;'H, Rr 


根据 行列 式 性 质 ， 有 


det(Q;  ) det(R;') = det(A,) = det(4)= det(P.' )det(D;') (E-10) 
于 是 
p, |x, r) =[(2)” det(P.)T "^ exp(-0.5|x, 一 有 Nie "di 
= A (x; P, N, , P.) 
MFE A (11-16) 导出 
p(x, | yi) x< PE |x, )p(x, | yii) =p(y, Jodi |x, )p(x, Fre) 
; > (F-1) 
=p(x, lya Dos |x,) 
s=] 
对 式 (F-1) 取 对 数 ， 得 到 其 等 价 算式 为 
S 
A = arg min(- In[p(x, | yia Jui pix) (F-3) 
k s-l 
容易 证 明 ，p(x; | y? A px, | yz) 可 以 通过 解析 递 推 求 得 ， 且 满足 高 斯 分 布 ， 即 
P(X; | Jui) =N (Xz Xa Po) (F-4) 
p(x, 1»ua)7 A (x, ua Paca) 
1 " T n-! ^ (F-5) 
E M doas as FK od. 
Gy IB. reu (x, — Xa) Paar 7 X )] 


式 中 
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Šiki x [x.p(x, | Ji: Xx, = IRENE | Xia xix, | px. | i-i 
= fF x, i p(x, Jade = FE, , 
Px- = E([Fx,_, + w, — Fš, Fx, +w, - F£,,]) 
= E([F(x, , - , 4) +w, ][F(x,_, — X, ,) + w, T} 
= FE[(x, , — X, Xa — X14 ) JF" + E[(w,w; ) 
- FP, ,F' +Q, 
又 因为 


S 
p, 1x) = TW os. ix, RD) 


s=] 


于 是 满足 式 (F-3) 的 x 等 价 于 


S 
i-o: - Hx) (RI) Qt - H'x,)] + 
k s=1 


8 e v ë 
ac Is 一 Xp )' P (x, 7 X,44)] = 0 
k 


S 
-J [Gr*)! (R (yi - H'x,)] + Pa iG, 73,4) = 0 


s=] 


S 
Pa a 7 & a) = 3 [GT ) (Ri) (yi - H'x,)] 


s=l 
$ 
-Y (Gr y (RD p; - H' (x, — Erpa ta) 


S 
= PEY (RD i - H2, 


s=1 


S 
- (Yr) qi) mo, - 43) 


S S 
{Pi +o ID) (a) B - 24979 [GT ) (Ri) Oi - H*44)] 
s-l s=] 
$ 
4 P,-[Pr, ,* (Hi) (Ri) H;] 乘 以 式 CF-12). 的 两 边 可 得 x 的 融合 估计 为 
s=1 


s 
X, = frka + F> GT XR;) Oi — H’ Šiki) 
s-l 


若 定义 
K, S (P + H'R'HY' H'i = PHAR 


对 上 式 进行 适当 的 变形 ， 得 


(F-6) 


(F-7) 


(F-8) 


(F-9) 


(F-10) 


(F-11) 


(F-12) 


(F-13) 


(F-14) 
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K, =P'H'R''(HP,, ,H! + RXHB,, ,H' + R). 
-P'(H'R'HP, ,H' + H'R''RXHP,, ,H' + Ry: 
-P'(H'RIOH Pu P, H (HP, H +R) (F-15) 
=P'(P')' P, ,H' (HP, ,H' + R): 
=P, H'(HP,, H` + R)* 
同 理 可 得 
P, = E((x, — X, )Xx, - X.) ) 
=E([x, — X. 7 K, (y - Hy )][x, — X4 7 K, (y - Hx, yn (F-16) 
= Pika- Pj, HK; - K HP, + K, (HP, ,H' + R)K, 
将 式 〈F-15) HHK, 代入 式 〈(F-16)， 并 采用 矩阵 求 逆 引 理化 简 ， 得 
P, =P — | E” (HP,, ,H* +R)” HP 


(F-17) 
=(P I +H'R' HY = P'° 
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